From 6e781c22891a137d7efc8feafa9bb1e4a2c2cc81 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 1 Jun 2019 11:43:16 -0400 Subject: [PATCH] InvenSense ICM20948 move to PX4Accelerometer, PX4Gyroscope, PX4Magnetometer helpers --- src/drivers/drv_sensor.h | 1 + src/drivers/imu/icm20948/CMakeLists.txt | 6 +- .../icm20948/{mag.cpp => ICM20948_mag.cpp} | 224 +----- .../imu/icm20948/{mag.h => ICM20948_mag.h} | 43 +- src/drivers/imu/icm20948/accel.cpp | 123 ---- src/drivers/imu/icm20948/accel.h | 67 -- src/drivers/imu/icm20948/gyro.cpp | 100 --- src/drivers/imu/icm20948/gyro.h | 66 -- src/drivers/imu/icm20948/icm20948.cpp | 647 ++++-------------- src/drivers/imu/icm20948/icm20948.h | 102 +-- src/drivers/imu/icm20948/icm20948_i2c.cpp | 2 + .../icm20948/{main.cpp => icm20948_main.cpp} | 85 +-- src/drivers/imu/icm20948/mag_i2c.cpp | 8 +- 13 files changed, 223 insertions(+), 1251 deletions(-) rename src/drivers/imu/icm20948/{mag.cpp => ICM20948_mag.cpp} (59%) rename src/drivers/imu/icm20948/{mag.h => ICM20948_mag.h} (87%) delete mode 100644 src/drivers/imu/icm20948/accel.cpp delete mode 100644 src/drivers/imu/icm20948/accel.h delete mode 100644 src/drivers/imu/icm20948/gyro.cpp delete mode 100644 src/drivers/imu/icm20948/gyro.h rename src/drivers/imu/icm20948/{main.cpp => icm20948_main.cpp} (81%) diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 4f5b23d3cc..f93216621e 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -61,6 +61,7 @@ #define DRV_MAG_DEVTYPE_RM3100 0x07 #define DRV_MAG_DEVTYPE_QMC5883 0x08 #define DRV_MAG_DEVTYPE_AK09916 0x09 +#define DRV_DEVTYPE_ICM20948 0x0A #define DRV_ACC_DEVTYPE_LSM303D 0x11 #define DRV_ACC_DEVTYPE_BMA180 0x12 #define DRV_ACC_DEVTYPE_MPU6000 0x13 diff --git a/src/drivers/imu/icm20948/CMakeLists.txt b/src/drivers/imu/icm20948/CMakeLists.txt index e32a67fa37..d956d14194 100644 --- a/src/drivers/imu/icm20948/CMakeLists.txt +++ b/src/drivers/imu/icm20948/CMakeLists.txt @@ -39,10 +39,8 @@ px4_add_module( icm20948.cpp icm20948_i2c.cpp icm20948_spi.cpp - main.cpp - accel.cpp - gyro.cpp - mag.cpp + icm20948_main.cpp + ICM20948_mag.cpp mag_i2c.cpp DEPENDS ) diff --git a/src/drivers/imu/icm20948/mag.cpp b/src/drivers/imu/icm20948/ICM20948_mag.cpp similarity index 59% rename from src/drivers/imu/icm20948/mag.cpp rename to src/drivers/imu/icm20948/ICM20948_mag.cpp index 002c046394..ccdbecc261 100644 --- a/src/drivers/imu/icm20948/mag.cpp +++ b/src/drivers/imu/icm20948/ICM20948_mag.cpp @@ -40,56 +40,34 @@ * */ -#include "mag.h" -#include "icm20948.h" +#include +#include +#include +#include +#include +#include "ICM20948_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), +ICM20948_mag::ICM20948_mag(ICM20948 *parent, device::Device *interface, enum Rotation rotation) : _interface(interface), + _px4_mag(parent->_interface->get_device_id(), (parent->_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), + rotation), _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{} + _mag_reads(perf_alloc(PC_COUNT, "icm20948: mag_reads")), + _mag_errors(perf_alloc(PC_COUNT, "icm20948: mag_errors")), + _mag_overruns(perf_alloc(PC_COUNT, "icm20948: mag_overruns")), + _mag_overflows(perf_alloc(PC_COUNT, "icm20948: mag_overflows")), + _mag_duplicates(perf_alloc(PC_COUNT, "icm20948: mag_duplicates")) { - // 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; + _px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK09916); + _px4_mag.set_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); @@ -97,43 +75,6 @@ ICM20948_mag::~ICM20948_mag() 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) { @@ -149,27 +90,25 @@ bool ICM20948_mag::check_duplicate(uint8_t *mag_data) 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)); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + int 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); + _measure(timestamp_sample, raw_data.ak8963_data); } } void -ICM20948_mag::_measure(struct ak8963_regs data) +ICM20948_mag::_measure(hrt_abstime timestamp_sample, ak8963_regs data) { - bool mag_notify = true; - if (check_duplicate((uint8_t *)&data.x) && !(data.st1 & 0x02)) { perf_count(_mag_duplicates); return; @@ -188,119 +127,14 @@ ICM20948_mag::_measure(struct ak8963_regs data) 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; - } + _px4_mag.set_external(_parent->is_external()); + _px4_mag.set_temperature(_parent->_last_temperature); + _px4_mag.set_error_count(perf_event_count(_mag_errors)); /* - * Align axes - note the accel & gryo are also re-aligned so this - * doesn't look obvious with the datasheet + * Align axes - Keeping consistent with the accel and gyro axes of the ICM20948 here, just aligning the magnetometer to them. */ - 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); - } + _px4_mag.update(timestamp_sample, data.y, data.x, -data.z); } void @@ -341,7 +175,7 @@ ICM20948_mag::passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size) uint8_t ICM20948_mag::read_reg(unsigned int reg) { - uint8_t buf; + uint8_t buf{}; if (_interface == nullptr) { passthrough_read(reg, &buf, 0x01); @@ -428,9 +262,7 @@ ICM20948_mag::ak8963_read_adjustments(void) } } - _mag_asa_x = ak8963_ASA[0]; - _mag_asa_y = ak8963_ASA[1]; - _mag_asa_z = ak8963_ASA[2]; + _px4_mag.set_sensitivity(ak8963_ASA[0], ak8963_ASA[1], ak8963_ASA[2]); return true; } diff --git a/src/drivers/imu/icm20948/mag.h b/src/drivers/imu/icm20948/ICM20948_mag.h similarity index 87% rename from src/drivers/imu/icm20948/mag.h rename to src/drivers/imu/icm20948/ICM20948_mag.h index b84a407ecb..bfb580ed6d 100644 --- a/src/drivers/imu/icm20948/mag.h +++ b/src/drivers/imu/icm20948/ICM20948_mag.h @@ -33,16 +33,12 @@ #pragma once -#include -#include -#include -#include -#include -#include +#include +#include +#include /* in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */ - -#define MPU9250_MAG_RANGE_GA 1.5e-3f; +static constexpr float MPU9250_MAG_RANGE_GA{1.5e-3f}; /* we are using the continuous fixed sampling rate of 100Hz */ @@ -132,15 +128,12 @@ typedef device::Device *(*ICM20948_mag_constructor)(int, bool); /** * Helper class implementing the magnetometer driver node. */ -class ICM20948_mag : public device::CDev +class ICM20948_mag { public: - ICM20948_mag(ICM20948 *parent, device::Device *interface, const char *path); + ICM20948_mag(ICM20948 *parent, device::Device *interface, enum Rotation rotation); ~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); @@ -152,8 +145,10 @@ public: bool ak8963_check_id(uint8_t &id); bool ak8963_read_adjustments(void); + void print_status() { _px4_mag.print_status(); } + protected: - Device *_interface; + device::Device *_interface; friend class ICM20948; @@ -161,7 +156,7 @@ protected: void measure(); /* Update the state with prefetched data (internally called by the regular measure() )*/ - void _measure(struct ak8963_regs data); + void _measure(hrt_abstime timestamp, struct ak8963_regs data); uint8_t read_reg(unsigned reg); void write_reg(unsigned reg, uint8_t value); @@ -169,27 +164,23 @@ protected: bool is_passthrough() { return _interface == nullptr; } private: + + PX4Magnetometer _px4_mag; + 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; + + bool _mag_reading_data{false}; + 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]; + uint8_t _last_mag_data[6] {}; /* do not allow to copy this class due to pointer data members */ ICM20948_mag(const ICM20948_mag &); diff --git a/src/drivers/imu/icm20948/accel.cpp b/src/drivers/imu/icm20948/accel.cpp deleted file mode 100644 index fba32ba746..0000000000 --- a/src/drivers/imu/icm20948/accel.cpp +++ /dev/null @@ -1,123 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file gyro.cpp - * - * Driver for the Invensense mpu9250 connected via SPI. - * - * @author Andrew Tridgell - * - * based on the mpu6000 driver - */ - -#include - -#include "accel.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 deleted file mode 100644 index ea1732b6a8..0000000000 --- a/src/drivers/imu/icm20948/accel.h +++ /dev/null @@ -1,67 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include - -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 deleted file mode 100644 index 3523499fb0..0000000000 --- a/src/drivers/imu/icm20948/gyro.cpp +++ /dev/null @@ -1,100 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file gyro.cpp - * - * Driver for the Invensense icm20948 connected via SPI. - * - * - * based on the mpu9250 driver - */ - -#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 deleted file mode 100644 index cff6c8c649..0000000000 --- a/src/drivers/imu/icm20948/gyro.h +++ /dev/null @@ -1,66 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include - -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 index 6027175f4e..e499fc7756 100644 --- a/src/drivers/imu/icm20948/icm20948.cpp +++ b/src/drivers/imu/icm20948/icm20948.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,9 +36,21 @@ * * 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 "ICM20948_mag.h" #include "icm20948.h" /* @@ -55,149 +67,54 @@ 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 -}; +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, +ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, const char *path, enum Rotation rotation, bool magnetometer_only) : ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), _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)), + _px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_DEFAULT : ORB_PRIO_HIGH), rotation), + _px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_DEFAULT : ORB_PRIO_HIGH), rotation), + _mag(this, mag_interface, rotation), _selected_bank(0xFF), // invalid/improbable bank value, will be set on first read/write _magnetometer_only(magnetometer_only), - _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) + _accel_reads(perf_alloc(PC_COUNT, "icm20948: acc_read")), + _gyro_reads(perf_alloc(PC_COUNT, "icm20948: gyro_read")), + _sample_perf(perf_alloc(PC_ELAPSED, "icm20948: read")), + _bad_transfers(perf_alloc(PC_COUNT, "icm20948: bad_trans")), + _bad_registers(perf_alloc(PC_COUNT, "icm20948: bad_reg")), + _good_transfers(perf_alloc(PC_COUNT, "icm20948: good_trans")), + _reset_retries(perf_alloc(PC_COUNT, "icm20948: reset")), + _duplicates(perf_alloc(PC_COUNT, "icm20948: dupe")) { - 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; + _px4_accel.set_device_type(DRV_DEVTYPE_ICM20948); + _px4_gyro.set_device_type(DRV_DEVTYPE_ICM20948); } ICM20948::~ICM20948() { - /* make sure we are truly inactive */ + // 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 */ + // delete the perf counter perf_free(_sample_perf); perf_free(_accel_reads); perf_free(_gyro_reads); @@ -211,8 +128,6 @@ ICM20948::~ICM20948() int ICM20948::init() { - irqstate_t state; - /* * 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 @@ -222,8 +137,6 @@ ICM20948::init() 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(); @@ -233,157 +146,38 @@ ICM20948::init() 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); + px4_usleep(100); - if (!_mag->is_passthrough() && _mag->_interface->init() != PX4_OK) { + 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(); + 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; - } - } + start(); 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: * @@ -394,21 +188,15 @@ int ICM20948::reset() 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(); + int ret = reset_mpu(); if (ret == OK && (_whoami == ICM_WHOAMI_20948)) { - ret = _mag->ak8963_reset(); + ret = _mag.ak8963_reset(); } - state = px4_enter_critical_section(); _reset_wait = hrt_absolute_time() + 10; - px4_leave_critical_section(state); return ret; } @@ -450,17 +238,15 @@ int ICM20948::reset_mpu() // 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; + _px4_gyro.set_scale(0.0174532 / 16.4); //1.0f / (32768.0f * (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 + write_checked_reg(ICMREG_20948_INT_ENABLE_1, BIT_RAW_RDY_EN); // INT: Raw data ready #ifdef USE_I2C - bool bypass = !_mag->is_passthrough(); + bool bypass = !_mag.is_passthrough(); #else bool bypass = false; #endif @@ -474,11 +260,9 @@ int ICM20948::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(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)); + write_checked_reg(ICMREG_20948_ACCEL_CONFIG_2, ICM_BITS_DEC3_CFG_32); retries = 3; bool all_ok = false; @@ -573,53 +357,6 @@ ICM20948::_set_sample_rate(unsigned desired_sample_rate_hz) } } -/* - * 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 interval = 1000000 / rate; - - /* check against maximum sane rate */ - if (interval < 1000) { - return -EINVAL; - } - - // adjust filters - float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f / interval; - _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 = interval; - - /* 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. */ @@ -635,39 +372,39 @@ ICM20948::_set_dlpf_filter(uint16_t frequency_hz) choose next highest filter frequency available for gyroscope */ if (frequency_hz == 0) { - _dlpf_freq_icm_gyro = 0; + //_dlpf_freq_icm_gyro = 0; filter = ICM_BITS_GYRO_DLPF_CFG_361HZ; } else if (frequency_hz <= 5) { - _dlpf_freq_icm_gyro = 5; + //_dlpf_freq_icm_gyro = 5; filter = ICM_BITS_GYRO_DLPF_CFG_5HZ; } else if (frequency_hz <= 11) { - _dlpf_freq_icm_gyro = 11; + //_dlpf_freq_icm_gyro = 11; filter = ICM_BITS_GYRO_DLPF_CFG_11HZ; } else if (frequency_hz <= 23) { - _dlpf_freq_icm_gyro = 23; + //_dlpf_freq_icm_gyro = 23; filter = ICM_BITS_GYRO_DLPF_CFG_23HZ; } else if (frequency_hz <= 51) { - _dlpf_freq_icm_gyro = 51; + //_dlpf_freq_icm_gyro = 51; filter = ICM_BITS_GYRO_DLPF_CFG_51HZ; } else if (frequency_hz <= 119) { - _dlpf_freq_icm_gyro = 119; + //_dlpf_freq_icm_gyro = 119; filter = ICM_BITS_GYRO_DLPF_CFG_119HZ; } else if (frequency_hz <= 151) { - _dlpf_freq_icm_gyro = 151; + //_dlpf_freq_icm_gyro = 151; filter = ICM_BITS_GYRO_DLPF_CFG_151HZ; } else if (frequency_hz <= 197) { - _dlpf_freq_icm_gyro = 197; + //_dlpf_freq_icm_gyro = 197; filter = ICM_BITS_GYRO_DLPF_CFG_197HZ; } else { - _dlpf_freq_icm_gyro = 0; + //_dlpf_freq_icm_gyro = 0; filter = ICM_BITS_GYRO_DLPF_CFG_361HZ; } @@ -677,35 +414,35 @@ ICM20948::_set_dlpf_filter(uint16_t frequency_hz) choose next highest filter frequency available for accelerometer */ if (frequency_hz == 0) { - _dlpf_freq_icm_accel = 0; + //_dlpf_freq_icm_accel = 0; filter = ICM_BITS_ACCEL_DLPF_CFG_473HZ; } else if (frequency_hz <= 5) { - _dlpf_freq_icm_accel = 5; + //_dlpf_freq_icm_accel = 5; filter = ICM_BITS_ACCEL_DLPF_CFG_5HZ; } else if (frequency_hz <= 11) { - _dlpf_freq_icm_accel = 11; + //_dlpf_freq_icm_accel = 11; filter = ICM_BITS_ACCEL_DLPF_CFG_11HZ; } else if (frequency_hz <= 23) { - _dlpf_freq_icm_accel = 23; + //_dlpf_freq_icm_accel = 23; filter = ICM_BITS_ACCEL_DLPF_CFG_23HZ; } else if (frequency_hz <= 50) { - _dlpf_freq_icm_accel = 50; + //_dlpf_freq_icm_accel = 50; filter = ICM_BITS_ACCEL_DLPF_CFG_50HZ; } else if (frequency_hz <= 111) { - _dlpf_freq_icm_accel = 111; + //_dlpf_freq_icm_accel = 111; filter = ICM_BITS_ACCEL_DLPF_CFG_111HZ; } else if (frequency_hz <= 246) { - _dlpf_freq_icm_accel = 246; + //_dlpf_freq_icm_accel = 246; filter = ICM_BITS_ACCEL_DLPF_CFG_246HZ; } else { - _dlpf_freq_icm_accel = 0; + //_dlpf_freq_icm_accel = 0; filter = ICM_BITS_ACCEL_DLPF_CFG_473HZ; } @@ -764,15 +501,10 @@ ICM20948::select_register_bank(uint8_t bank) uint8_t ICM20948::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); - } + select_register_bank(REG_BANK(reg)); + _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1); return buf; } @@ -780,35 +512,22 @@ ICM20948::read_reg(unsigned reg, uint32_t speed) 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); + if (buf == NULL) { + return PX4_ERROR; } - return ret; + select_register_bank(REG_BANK(start_reg)); + return _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count); } uint16_t ICM20948::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)); - } + select_register_bank(REG_BANK(reg)); + _interface->read(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &buf, arraySize(buf)); return (uint16_t)(buf[0] << 8) | buf[1]; } @@ -817,22 +536,14 @@ 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); - } + select_register_bank(REG_BANK(reg)); + _interface->write(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &value, 1); } void ICM20948::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) { - uint8_t val; - - val = read_reg(reg); + uint8_t val = read_reg(reg); val &= ~clearbits; val |= setbits; write_reg(reg, val); @@ -841,9 +552,7 @@ ICM20948::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) void ICM20948::modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) { - uint8_t val; - - val = read_reg(reg); + uint8_t val = read_reg(reg); val &= ~clearbits; val |= setbits; write_checked_reg(reg, val); @@ -868,27 +577,27 @@ ICM20948::set_accel_range(unsigned max_g_in) { uint8_t afs_sel; float lsb_per_g; - float max_accel_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; + //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; + //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; + //max_accel_g = 4; } else { // 2g - AFS_SEL = 0 afs_sel = 0; lsb_per_g = 16384; - max_accel_g = 2; + //max_accel_g = 2; } switch (_whoami) { @@ -897,8 +606,7 @@ ICM20948::set_accel_range(unsigned max_g_in) break; } - _accel_range_scale = (CONSTANTS_ONE_G / lsb_per_g); - _accel_range_m_s2 = max_accel_g * CONSTANTS_ONE_G; + _px4_accel.set_scale(CONSTANTS_ONE_G / lsb_per_g); return OK; } @@ -909,15 +617,7 @@ 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(); - - ScheduleOnInterval(_call_interval - MPU9250_TIMER_REDUCTION, 10000); + ScheduleOnInterval(_call_interval - MPU9250_TIMER_REDUCTION, 1000); } void @@ -968,12 +668,7 @@ ICM20948::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); - } + reset_mpu(); // after doing a reset we need to wait a long // time before we do any other register writes @@ -1044,7 +739,6 @@ bool ICM20948::check_duplicate(uint8_t *accel_data) void ICM20948::measure() { - if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete return; @@ -1067,11 +761,12 @@ ICM20948::measure() /* start measuring */ perf_begin(_sample_perf); + const hrt_abstime timestamp_sample = hrt_absolute_time(); /* * Fetch the full set of measurements from the MPU9250 in one pass */ - if ((!_magnetometer_only || _mag->is_passthrough()) && _register_wait == 0) { + if ((!_magnetometer_only || _mag.is_passthrough()) && _register_wait == 0) { select_register_bank(REG_BANK(ICMREG_20948_ACCEL_XOUT_H)); @@ -1095,15 +790,15 @@ ICM20948::measure() # ifdef USE_I2C - if (_mag->is_passthrough()) { + if (_mag.is_passthrough()) { # endif - _mag->_measure(mpu_report.mag); + _mag._measure(timestamp_sample, mpu_report.mag); # ifdef USE_I2C } else { - _mag->measure(); + _mag.measure(); } # endif @@ -1155,6 +850,8 @@ ICM20948::measure() */ _last_temperature = (report.temp) / 333.87f + 21.0f; + _px4_accel.set_temperature(_last_temperature); + _px4_gyro.set_temperature(_last_temperature); /* * Convert and publish accelerometer and gyrometer data. @@ -1164,43 +861,30 @@ ICM20948::measure() /* * 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); - - /* - * Apply the swap - */ - report.accel_x = accel_xt; - report.accel_y = accel_yt; - report.gyro_x = gyro_xt; - report.gyro_y = gyro_yt; - } + int16_t gyro_xt = report.gyro_y; + int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); /* - * Report buffers. + * Apply the swap */ - sensor_accel_s arb; - sensor_gyro_s grb; - - /* - * Adjust and scale results to m/s^2. - */ - grb.timestamp = arb.timestamp = hrt_absolute_time(); + report.accel_x = accel_xt; + report.accel_y = accel_yt; + report.gyro_x = gyro_xt; + report.gyro_y = gyro_yt; // report the error count as the sum of the number of bad // transfers and bad register reads. This allows the higher // level code to decide if it should use this sensor based on // whether it has had failures - grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + _px4_accel.set_error_count(error_count); + _px4_gyro.set_error_count(error_count); /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1218,96 +902,8 @@ ICM20948::measure() */ /* 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); - } + _px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z); + _px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z); } /* stop measuring */ @@ -1317,7 +913,6 @@ ICM20948::measure() 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); @@ -1326,11 +921,11 @@ ICM20948::print_info() 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"); + _px4_accel.print_status(); + _px4_gyro.print_status(); } + + _mag.print_status(); } diff --git a/src/drivers/imu/icm20948/icm20948.h b/src/drivers/imu/icm20948/icm20948.h index 939b507346..5814083b79 100644 --- a/src/drivers/imu/icm20948/icm20948.h +++ b/src/drivers/imu/icm20948/icm20948.h @@ -31,27 +31,24 @@ * ****************************************************************************/ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#pragma once + +#include + +#include #include + +#include + +#include +#include +#include #include -#include +#include + #include -#include "accel.h" -#include "gyro.h" -#include "mag.h" - +#include "ICM20948_mag.h" #if defined(PX4_I2C_OBDEV_MPU9250) || defined(PX4_I2C_BUS_EXPANSION) # define USE_I2C @@ -366,21 +363,17 @@ 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 px4::ScheduledWorkItem { public: - ICM20948(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, - const char *path_mag, - enum Rotation rotation, + ICM20948(device::Device *interface, device::Device *mag_interface, const char *path, enum Rotation rotation, bool magnetometer_only); virtual ~ICM20948(); virtual int init(); - uint8_t get_whoami(); + uint8_t get_whoami() { return _whoami; } /** * Diagnostics - print some basic information about the driver. @@ -393,40 +386,28 @@ protected: virtual int probe(); - friend class ICM20948_accel; friend class ICM20948_mag; - friend class ICM20948_gyro; void Run() override; private: - ICM20948_accel *_accel; - ICM20948_gyro *_gyro; - ICM20948_mag *_mag; + + PX4Accelerometer _px4_accel; + PX4Gyroscope _px4_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) */ - 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 _call_interval{1000}; unsigned _dlpf_freq; unsigned _dlpf_freq_icm_gyro; unsigned _dlpf_freq_icm_accel; - unsigned _sample_rate; + unsigned _sample_rate{1000}; + perf_counter_t _accel_reads; perf_counter_t _gyro_reads; perf_counter_t _sample_perf; @@ -436,48 +417,32 @@ private: 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; + uint8_t _register_wait{0}; + uint64_t _reset_wait{0}; // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#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; + const uint16_t *_checked_registers{nullptr}; uint8_t _checked_values[ICM20948_NUM_CHECKED_REGISTERS]; uint8_t _checked_bad[ICM20948_NUM_CHECKED_REGISTERS]; - unsigned _checked_next; - unsigned _num_checked_registers; + unsigned _checked_next{0}; + unsigned _num_checked_registers{0}; // last temperature reading for print_info() - float _last_temperature; + float _last_temperature{0.0f}; 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; + uint8_t _last_accel_data[6] {}; + bool _got_duplicate{false}; /** * Start automatic measurement. @@ -619,7 +584,4 @@ private: */ 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 index 732cb096bd..9baba07575 100644 --- a/src/drivers/imu/icm20948/icm20948_i2c.cpp +++ b/src/drivers/imu/icm20948/icm20948_i2c.cpp @@ -37,7 +37,9 @@ * I2C interface for ICM20948 */ +#include #include +#include #include "icm20948.h" diff --git a/src/drivers/imu/icm20948/main.cpp b/src/drivers/imu/icm20948/icm20948_main.cpp similarity index 81% rename from src/drivers/imu/icm20948/main.cpp rename to src/drivers/imu/icm20948/icm20948_main.cpp index 642a6af531..6c3bca8e7e 100644 --- a/src/drivers/imu/icm20948/main.cpp +++ b/src/drivers/imu/icm20948/icm20948_main.cpp @@ -39,11 +39,18 @@ * based on the mpu9250 driver */ +#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" +#define ICM_DEVICE_PATH_EXT "/dev/icm20948_ext" /** driver 'main' command */ extern "C" { __EXPORT int icm20948_main(int argc, char *argv[]); } @@ -69,19 +76,17 @@ namespace icm20948 struct icm20948_bus_option { enum ICM20948_BUS busid; - const char *accelpath; - const char *gyropath; - const char *magpath; + const char *path; ICM20948_constructor interface_constructor; bool magpassthrough; uint8_t busnum; uint32_t address; - ICM20948 *dev; + 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 }, + { ICM20948_BUS_I2C_EXTERNAL, ICM_DEVICE_PATH_EXT, &ICM20948_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_EXT_ICM20948_1, nullptr }, #endif #endif @@ -95,7 +100,6 @@ void start(enum ICM20948_BUS busid, enum Rotation rotation, bool external_bus, b 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(); @@ -120,8 +124,6 @@ struct icm20948_bus_option &find_bus(enum ICM20948_BUS busid) 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) { @@ -154,8 +156,7 @@ start_bus(struct icm20948_bus_option &bus, enum Rotation rotation, bool external #endif - bus.dev = new ICM20948(interface, mag_interface, bus.accelpath, bus.gyropath, bus.magpath, rotation, - magnetometer_only); + bus.dev = new ICM20948(interface, mag_interface, bus.path, rotation, magnetometer_only); if (bus.dev == nullptr) { delete interface; @@ -171,33 +172,10 @@ start_bus(struct icm20948_bus_option &bus, enum Rotation rotation, bool external 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; @@ -256,32 +234,6 @@ stop(enum ICM20948_BUS busid) 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. */ @@ -304,7 +256,7 @@ info(enum ICM20948_BUS busid) void usage() { - PX4_INFO("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'testerror'"); + PX4_INFO("missing command: try 'start', 'info', 'stop',\n 'regdump', 'testerror'"); PX4_INFO("options:"); PX4_INFO(" -X (i2c external bus)"); PX4_INFO(" -I (i2c internal bus)"); @@ -383,13 +335,6 @@ icm20948_main(int argc, char *argv[]) icm20948::stop(busid); } - /* - * Reset the driver. - */ - if (!strcmp(verb, "reset")) { - icm20948::reset(busid); - } - /* * Print driver information. */ diff --git a/src/drivers/imu/icm20948/mag_i2c.cpp b/src/drivers/imu/icm20948/mag_i2c.cpp index 87a04a9453..be38bf1547 100644 --- a/src/drivers/imu/icm20948/mag_i2c.cpp +++ b/src/drivers/imu/icm20948/mag_i2c.cpp @@ -38,7 +38,9 @@ */ #include "icm20948.h" -#include "mag.h" +#include "ICM20948_mag.h" + +#include #ifdef USE_I2C @@ -73,7 +75,7 @@ AK8963_I2C::AK8963_I2C(int bus) : int AK8963_I2C::write(unsigned reg_speed, void *data, unsigned count) { - uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE]; + uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE] {}; if (sizeof(cmd) < (count + 1)) { return -EIO; @@ -108,4 +110,4 @@ AK8963_I2C::probe() return OK; } -#endif +#endif // USE_I2C