From a7469d1a688bc77845d230df9aecab7d993c13e7 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 24 May 2019 13:54:22 -0400 Subject: [PATCH] InvenSense MPU9250 move to PX4Accelerometer and PX4Gyroscope helpers --- .../mpu9250/{mag_i2c.cpp => AK8963_I2C.cpp} | 8 +- src/drivers/imu/mpu9250/CMakeLists.txt | 11 +- .../imu/mpu9250/{mag.cpp => MPU9250_mag.cpp} | 191 +------ .../imu/mpu9250/{mag.h => MPU9250_mag.h} | 41 +- src/drivers/imu/mpu9250/accel.cpp | 147 ------ src/drivers/imu/mpu9250/accel.h | 63 --- src/drivers/imu/mpu9250/gyro.cpp | 113 ----- src/drivers/imu/mpu9250/gyro.h | 62 --- src/drivers/imu/mpu9250/mpu9250.cpp | 469 ++---------------- src/drivers/imu/mpu9250/mpu9250.h | 97 +--- src/drivers/imu/mpu9250/mpu9250_i2c.cpp | 3 +- .../mpu9250/{main.cpp => mpu9250_main.cpp} | 133 +---- src/drivers/imu/mpu9250/mpu9250_spi.cpp | 5 +- .../drivers/magnetometer/PX4Magnetometer.cpp | 4 +- .../drivers/magnetometer/PX4Magnetometer.hpp | 3 + 15 files changed, 135 insertions(+), 1215 deletions(-) rename src/drivers/imu/mpu9250/{mag_i2c.cpp => AK8963_I2C.cpp} (93%) rename src/drivers/imu/mpu9250/{mag.cpp => MPU9250_mag.cpp} (66%) rename src/drivers/imu/mpu9250/{mag.h => MPU9250_mag.h} (88%) delete mode 100644 src/drivers/imu/mpu9250/accel.cpp delete mode 100644 src/drivers/imu/mpu9250/accel.h delete mode 100644 src/drivers/imu/mpu9250/gyro.cpp delete mode 100644 src/drivers/imu/mpu9250/gyro.h rename src/drivers/imu/mpu9250/{main.cpp => mpu9250_main.cpp} (67%) diff --git a/src/drivers/imu/mpu9250/mag_i2c.cpp b/src/drivers/imu/mpu9250/AK8963_I2C.cpp similarity index 93% rename from src/drivers/imu/mpu9250/mag_i2c.cpp rename to src/drivers/imu/mpu9250/AK8963_I2C.cpp index 91e74bf0a833..2966c815d486 100644 --- a/src/drivers/imu/mpu9250/mag_i2c.cpp +++ b/src/drivers/imu/mpu9250/AK8963_I2C.cpp @@ -39,11 +39,10 @@ #include #include -#include #include #include "mpu9250.h" -#include "mag.h" +#include "MPU9250_mag.h" #ifdef USE_I2C @@ -69,10 +68,9 @@ 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) +AK8963_I2C::AK8963_I2C(int bus) : I2C("AK8963_I2C", nullptr, bus, AK8963_I2C_ADDR, 400000) { - _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250; + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250; } int diff --git a/src/drivers/imu/mpu9250/CMakeLists.txt b/src/drivers/imu/mpu9250/CMakeLists.txt index 833d2e8e5842..747decb21a1d 100644 --- a/src/drivers/imu/mpu9250/CMakeLists.txt +++ b/src/drivers/imu/mpu9250/CMakeLists.txt @@ -36,14 +36,15 @@ px4_add_module( STACK_MAIN 1500 COMPILE_FLAGS SRCS + AK8963_I2C.cpp mpu9250.cpp mpu9250_i2c.cpp + MPU9250_mag.cpp + mpu9250_main.cpp mpu9250_spi.cpp - main.cpp - accel.cpp - gyro.cpp - mag.cpp - mag_i2c.cpp DEPENDS px4_work_queue + drivers_accelerometer + drivers_gyroscope + drivers_magnetometer ) diff --git a/src/drivers/imu/mpu9250/mag.cpp b/src/drivers/imu/mpu9250/MPU9250_mag.cpp similarity index 66% rename from src/drivers/imu/mpu9250/mag.cpp rename to src/drivers/imu/mpu9250/MPU9250_mag.cpp index 84130148e021..0e8ac242d756 100644 --- a/src/drivers/imu/mpu9250/mag.cpp +++ b/src/drivers/imu/mpu9250/MPU9250_mag.cpp @@ -45,65 +45,29 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "mag.h" -#include "mpu9250.h" +#include "MPU9250_mag.h" +#include "mpu9250.h" // If interface is non-null, then it will used for interacting with the device. // Otherwise, it will passthrough the parent MPU9250 -MPU9250_mag::MPU9250_mag(MPU9250 *parent, device::Device *interface, const char *path) : - CDev("MPU9250_mag", path), +MPU9250_mag::MPU9250_mag(MPU9250 *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_duplicates(perf_alloc(PC_COUNT, "mpu9250_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_MPU9250); + _px4_mag.set_scale(MPU9250_MAG_RANGE_GA); } MPU9250_mag::~MPU9250_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); @@ -111,45 +75,6 @@ MPU9250_mag::~MPU9250_mag() perf_free(_mag_duplicates); } -int -MPU9250_mag::init() -{ - int ret = CDev::init(); - - /* if cdev init failed, bail now */ - if (ret != OK) { - if (_parent->_whoami == MPU_WHOAMI_9250) { - PX4_ERR("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 MPU9250_mag::check_duplicate(uint8_t *mag_data) { if (memcmp(mag_data, &_last_mag_data, sizeof(_last_mag_data)) == 0) { @@ -170,18 +95,17 @@ MPU9250_mag::measure() struct ak09916_regs ak09916_data; } raw_data; - uint8_t ret = _interface->read(AK8963REG_ST1, &raw_data, sizeof(struct ak8963_regs)); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + int ret = _interface->read(AK8963REG_ST1, &raw_data, sizeof(struct ak8963_regs)); if (ret == OK) { - _measure(raw_data.ak8963_data); + _measure(timestamp_sample, raw_data.ak8963_data); } } void -MPU9250_mag::_measure(struct ak8963_regs data) +MPU9250_mag::_measure(hrt_abstime timestamp_sample, struct ak8963_regs data) { - bool mag_notify = true; - if (check_duplicate((uint8_t *)&data.x) && !(data.st1 & 0x02)) { perf_count(_mag_duplicates); return; @@ -200,92 +124,15 @@ MPU9250_mag::_measure(struct ak8963_regs data) perf_count(_mag_overflows); } - mag_report mrb; - mrb.timestamp = hrt_absolute_time(); - - mrb.is_external = _parent->is_external(); + _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 */ - float xraw_f, yraw_f, zraw_f; - - 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); - - 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 -MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - /* - * Repeated in MPU9250_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.x, -data.y, -data.z); } void @@ -326,7 +173,7 @@ MPU9250_mag::passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size) uint8_t MPU9250_mag::read_reg(unsigned int reg) { - uint8_t buf; + uint8_t buf{}; if (_interface == nullptr) { passthrough_read(reg, &buf, 0x01); @@ -413,9 +260,7 @@ MPU9250_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/mpu9250/mag.h b/src/drivers/imu/mpu9250/MPU9250_mag.h similarity index 88% rename from src/drivers/imu/mpu9250/mag.h rename to src/drivers/imu/mpu9250/MPU9250_mag.h index a4ad3dc713b7..d340b0ef8a50 100644 --- a/src/drivers/imu/mpu9250/mag.h +++ b/src/drivers/imu/mpu9250/MPU9250_mag.h @@ -33,20 +33,18 @@ #pragma once -#include "drivers/device/ringbuffer.h" // ringbuffer::RingBuffer -#include "drivers/drv_mag.h" // mag_calibration_s +#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 */ - #define MPU9250_AK8963_SAMPLE_RATE 100 /* ak8963 register address and bit definitions */ - #define AK8963_I2C_ADDR 0x0C #define AK8963_DEVICE_ID 0x48 @@ -128,15 +126,12 @@ typedef device::Device *(*MPU9250_mag_constructor)(int, bool); /** * Helper class implementing the magnetometer driver node. */ -class MPU9250_mag : public device::CDev +class MPU9250_mag { public: - MPU9250_mag(MPU9250 *parent, device::Device *interface, const char *path); + MPU9250_mag(MPU9250 *parent, device::Device *interface, enum Rotation rotation); ~MPU9250_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); @@ -148,8 +143,10 @@ class MPU9250_mag : public device::CDev 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 MPU9250; @@ -157,7 +154,7 @@ class MPU9250_mag : public device::CDev 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); @@ -165,27 +162,23 @@ class MPU9250_mag : public device::CDev bool is_passthrough() { return _interface == nullptr; } private: + + PX4Magnetometer _px4_mag; + MPU9250 *_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 */ MPU9250_mag(const MPU9250_mag &); diff --git a/src/drivers/imu/mpu9250/accel.cpp b/src/drivers/imu/mpu9250/accel.cpp deleted file mode 100644 index 230c5f006b30..000000000000 --- a/src/drivers/imu/mpu9250/accel.cpp +++ /dev/null @@ -1,147 +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 - -#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 "mpu9250.h" - -MPU9250_accel::MPU9250_accel(MPU9250 *parent, const char *path) : - CDev("MPU9250_accel", path), - _parent(parent) -{ -} - -MPU9250_accel::~MPU9250_accel() -{ - if (_accel_class_instance != -1) { - unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); - } -} - -int -MPU9250_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 -MPU9250_accel::parent_poll_notify() -{ - poll_notify(POLLIN); -} - -int -MPU9250_accel::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - /* - * Repeated in MPU9250_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/mpu9250/accel.h b/src/drivers/imu/mpu9250/accel.h deleted file mode 100644 index fd9cc63eda62..000000000000 --- a/src/drivers/imu/mpu9250/accel.h +++ /dev/null @@ -1,63 +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 - -class MPU9250; - -/** - * Helper class implementing the accel driver node. - */ -class MPU9250_accel : public device::CDev -{ -public: - MPU9250_accel(MPU9250 *parent, const char *path); - ~MPU9250_accel(); - - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - virtual int init(); - -protected: - friend class MPU9250; - - void parent_poll_notify(); - -private: - MPU9250 *_parent; - - orb_advert_t _accel_topic{nullptr}; - int _accel_orb_class_instance{-1}; - int _accel_class_instance{-1}; - -}; diff --git a/src/drivers/imu/mpu9250/gyro.cpp b/src/drivers/imu/mpu9250/gyro.cpp deleted file mode 100644 index 465bbae5aee6..000000000000 --- a/src/drivers/imu/mpu9250/gyro.cpp +++ /dev/null @@ -1,113 +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 -#include -#include -#include -#include -#include -#include -#include -#include - -#include "mag.h" -#include "gyro.h" -#include "mpu9250.h" - -MPU9250_gyro::MPU9250_gyro(MPU9250 *parent, const char *path) : - CDev("MPU9250_gyro", path), - _parent(parent) -{ -} - -MPU9250_gyro::~MPU9250_gyro() -{ - if (_gyro_class_instance != -1) { - unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance); - } -} - -int -MPU9250_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 -MPU9250_gyro::parent_poll_notify() -{ - poll_notify(POLLIN); -} - -int -MPU9250_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/mpu9250/gyro.h b/src/drivers/imu/mpu9250/gyro.h deleted file mode 100644 index d24088d23f60..000000000000 --- a/src/drivers/imu/mpu9250/gyro.h +++ /dev/null @@ -1,62 +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 - -class MPU9250; - -/** - * Helper class implementing the gyro driver node. - */ -class MPU9250_gyro : public device::CDev -{ -public: - MPU9250_gyro(MPU9250 *parent, const char *path); - ~MPU9250_gyro(); - - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - virtual int init(); - -protected: - friend class MPU9250; - - void parent_poll_notify(); - -private: - MPU9250 *_parent; - - orb_advert_t _gyro_topic{nullptr}; - int _gyro_orb_class_instance{-1}; - int _gyro_class_instance{-1}; -}; diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index 913ce7d54773..adb8790a253c 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -49,17 +49,9 @@ #include #include #include -#include -#include -#include -#include -#include -#include #include -#include "mag.h" -#include "accel.h" -#include "gyro.h" +#include "MPU9250_mag.h" #include "mpu9250.h" /* @@ -89,30 +81,16 @@ const uint16_t MPU9250::_mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS MPUREG_INT_PIN_CFG }; -MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, - const char *path_gyro, const char *path_mag, - enum Rotation rotation, +MPU9250::MPU9250(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 MPU9250_accel(this, path_accel)), - _gyro(magnetometer_only ? nullptr : new MPU9250_gyro(this, path_gyro)), - _mag(new MPU9250_mag(this, mag_interface, path_mag)), + _px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation), + _px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : 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), - _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")), @@ -120,95 +98,16 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const _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) + _duplicates(perf_alloc(PC_COUNT, "mpu9250_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_ACC_DEVTYPE_MPU9250); + _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_MPU9250); } MPU9250::~MPU9250() { /* 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); @@ -224,8 +123,6 @@ MPU9250::~MPU9250() int MPU9250::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 @@ -235,8 +132,6 @@ MPU9250::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(); @@ -246,89 +141,13 @@ MPU9250::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 */ if (_whoami == MPU_WHOAMI_9250) { @@ -336,22 +155,13 @@ MPU9250::init() up_udelay(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"); @@ -359,47 +169,13 @@ MPU9250::init() } } - 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 MPU9250::get_whoami() -{ - return _whoami; -} - int MPU9250::reset() { - irqstate_t state; - /* When the mpu9250 starts from 0V the internal power on circuit * per the data sheet will require: * @@ -410,22 +186,15 @@ int MPU9250::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 == MPU_WHOAMI_9250)) { - 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; } @@ -467,8 +236,7 @@ int MPU9250::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); @@ -476,7 +244,7 @@ int MPU9250::reset_mpu() write_checked_reg(MPUREG_INT_ENABLE, 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 @@ -578,53 +346,6 @@ MPU9250::_set_sample_rate(unsigned desired_sample_rate_hz) } } -/* - * Set poll rate - */ -int -MPU9250::_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. */ @@ -723,9 +444,7 @@ MPU9250::write_reg(unsigned reg, uint8_t value) void MPU9250::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); @@ -734,9 +453,7 @@ MPU9250::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) void MPU9250::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); @@ -761,27 +478,27 @@ MPU9250::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) { @@ -791,8 +508,7 @@ MPU9250::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; } @@ -803,14 +519,6 @@ MPU9250::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, 1000); } @@ -934,7 +642,6 @@ bool MPU9250::check_duplicate(uint8_t *accel_data) void MPU9250::measure() { - if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete return; @@ -955,11 +662,13 @@ MPU9250::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) { if (_whoami == MPU_WHOAMI_9250 || _whoami == MPU_WHOAMI_6500) { if (OK != read_reg_range(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&mpu_report, sizeof(mpu_report))) { perf_end(_sample_perf); @@ -982,17 +691,17 @@ MPU9250::measure() # ifdef USE_I2C - if (_mag->is_passthrough()) { + if (_mag.is_passthrough()) { # endif if (_register_wait == 0) { - _mag->_measure(mpu_report.mag); + _mag._measure(timestamp_sample, mpu_report.mag); } # ifdef USE_I2C } else { - _mag->measure(); + _mag.measure(); } # endif @@ -1033,6 +742,8 @@ MPU9250::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. @@ -1058,22 +769,13 @@ MPU9250::measure() 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); + 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. @@ -1091,96 +793,8 @@ MPU9250::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 */ @@ -1190,7 +804,6 @@ MPU9250::measure() void MPU9250::print_info() { - ::printf("Device type:%d\n", _whoami); perf_print_counter(_sample_perf); perf_print_counter(_accel_reads); perf_print_counter(_gyro_reads); @@ -1199,11 +812,11 @@ MPU9250::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/mpu9250/mpu9250.h b/src/drivers/imu/mpu9250/mpu9250.h index 9987464c8bdc..d47ee5ad30b8 100644 --- a/src/drivers/imu/mpu9250/mpu9250.h +++ b/src/drivers/imu/mpu9250/mpu9250.h @@ -35,26 +35,14 @@ #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" - +#include "MPU9250_mag.h" #if defined(PX4_I2C_OBDEV_MPU9250) || defined(PX4_I2C_BUS_EXPANSION) # define USE_I2C @@ -248,21 +236,17 @@ extern int MPU9250_probe(device::Device *dev); typedef device::Device *(*MPU9250_constructor)(int, uint32_t, bool); class MPU9250_mag; -class MPU9250_accel; -class MPU9250_gyro; class MPU9250 : public px4::ScheduledWorkItem { public: - MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, - const char *path_mag, - enum Rotation rotation, + MPU9250(device::Device *interface, device::Device *mag_interface, const char *path, enum Rotation rotation, bool magnetometer_only); virtual ~MPU9250(); virtual int init(); - uint8_t get_whoami(); + uint8_t get_whoami() { return _whoami; } /** * Diagnostics - print some basic information about the driver. @@ -271,42 +255,30 @@ class MPU9250 : public px4::ScheduledWorkItem protected: device::Device *_interface; - uint8_t _whoami; /** whoami result */ + uint8_t _whoami{0}; /** whoami result */ virtual int probe(); - friend class MPU9250_accel; friend class MPU9250_mag; - friend class MPU9250_gyro; void Run() override; private: - MPU9250_accel *_accel; - MPU9250_gyro *_gyro; - MPU9250_mag *_mag; + + PX4Accelerometer _px4_accel; + PX4Gyroscope _px4_gyro; + + MPU9250_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 _sample_rate; + unsigned _sample_rate{1000}; + perf_counter_t _accel_reads; perf_counter_t _gyro_reads; perf_counter_t _sample_perf; @@ -316,48 +288,32 @@ class MPU9250 : public px4::ScheduledWorkItem 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 MPU9250_NUM_CHECKED_REGISTERS{11}; static const uint16_t _mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS]; - const uint16_t *_checked_registers; + const uint16_t *_checked_registers{nullptr}; - uint8_t _checked_values[MPU9250_NUM_CHECKED_REGISTERS]; - uint8_t _checked_bad[MPU9250_NUM_CHECKED_REGISTERS]; - unsigned _checked_next; - unsigned _num_checked_registers; + uint8_t _checked_values[MPU9250_NUM_CHECKED_REGISTERS] {}; + uint8_t _checked_bad[MPU9250_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. @@ -477,11 +433,6 @@ class MPU9250 : public px4::ScheduledWorkItem */ 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 */ diff --git a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp b/src/drivers/imu/mpu9250/mpu9250_i2c.cpp index 8a856f9ce16b..af21bd309df8 100644 --- a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp +++ b/src/drivers/imu/mpu9250/mpu9250_i2c.cpp @@ -39,7 +39,6 @@ #include #include -#include #include #include "mpu9250.h" @@ -79,7 +78,7 @@ MPU9250_I2C::MPU9250_I2C(int bus, uint32_t address) : int MPU9250_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; diff --git a/src/drivers/imu/mpu9250/main.cpp b/src/drivers/imu/mpu9250/mpu9250_main.cpp similarity index 67% rename from src/drivers/imu/mpu9250/main.cpp rename to src/drivers/imu/mpu9250/mpu9250_main.cpp index ed2843106441..b775f2b9bbf8 100644 --- a/src/drivers/imu/mpu9250/main.cpp +++ b/src/drivers/imu/mpu9250/mpu9250_main.cpp @@ -43,53 +43,21 @@ */ #include - -#include -#include -#include -#include -#include -#include -#include #include - -#include -#include -#include - +#include +#include #include #include - #include -#include -#include -#include -#include -#include -#include #include #include "mpu9250.h" -#define MPU_DEVICE_PATH_ACCEL "/dev/mpu9250_accel" -#define MPU_DEVICE_PATH_GYRO "/dev/mpu9250_gyro" -#define MPU_DEVICE_PATH_MAG "/dev/mpu9250_mag" - -#define MPU_DEVICE_PATH_ACCEL_1 "/dev/mpu9250_accel1" -#define MPU_DEVICE_PATH_GYRO_1 "/dev/mpu9250_gyro1" -#define MPU_DEVICE_PATH_MAG_1 "/dev/mpu9250_mag1" - -#define MPU_DEVICE_PATH_ACCEL_EXT "/dev/mpu9250_accel_ext" -#define MPU_DEVICE_PATH_GYRO_EXT "/dev/mpu9250_gyro_ext" -#define MPU_DEVICE_PATH_MAG_EXT "/dev/mpu9250_mag_ext" - -#define MPU_DEVICE_PATH_ACCEL_EXT1 "/dev/mpu9250_accel_ext1" -#define MPU_DEVICE_PATH_GYRO_EXT1 "/dev/mpu9250_gyro_ext1" -#define MPU_DEVICE_PATH_MAG_EXT1 "/dev/mpu9250_mag_ext1" - -#define MPU_DEVICE_PATH_ACCEL_EXT2 "/dev/mpu9250_accel_ext2" -#define MPU_DEVICE_PATH_GYRO_EXT2 "/dev/mpu9250_gyro_ext2" -#define MPU_DEVICE_PATH_MAG_EXT2 "/dev/mpu9250_mag_ext2" +#define MPU_DEVICE_PATH "/dev/mpu9250" +#define MPU_DEVICE_PATH_1 "/dev/mpu9250_1" +#define MPU_DEVICE_PATH_EXT "/dev/mpu9250_ext" +#define MPU_DEVICE_PATH_EXT_1 "/dev/mpu9250_ext_1" +#define MPU_DEVICE_PATH_EXT_2 "/dev/mpu9250_ext_2" /** driver 'main' command */ extern "C" { __EXPORT int mpu9250_main(int argc, char *argv[]); } @@ -115,9 +83,7 @@ namespace mpu9250 struct mpu9250_bus_option { enum MPU9250_BUS busid; - const char *accelpath; - const char *gyropath; - const char *magpath; + const char *path; MPU9250_constructor interface_constructor; bool magpassthrough; uint8_t busnum; @@ -126,28 +92,28 @@ struct mpu9250_bus_option { } bus_options[] = { #if defined (USE_I2C) # if defined(PX4_I2C_BUS_ONBOARD) && defined(PX4_I2C_OBDEV_MPU9250) - { MPU9250_BUS_I2C_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_I2C_interface, false, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_MPU9250, nullptr }, + { MPU9250_BUS_I2C_INTERNAL, MPU_DEVICE_PATH, &MPU9250_I2C_interface, false, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_MPU9250, nullptr }, # endif # if defined(PX4_I2C_BUS_EXPANSION) # 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 }, + { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_EXT, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_OBDEV_MPU9250, nullptr }, # endif #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 }, + { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_EXT_1, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION1, PX4_I2C_OBDEV_MPU9250, nullptr }, # endif # if defined(PX4_I2C_BUS_EXPANSION2) && defined(PX4_I2C_OBDEV_MPU9250) - { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT2, MPU_DEVICE_PATH_GYRO_EXT2, MPU_DEVICE_PATH_MAG_EXT2, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION2, PX4_I2C_OBDEV_MPU9250, nullptr }, + { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_EXT_2, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION2, PX4_I2C_OBDEV_MPU9250, nullptr }, # endif #endif #ifdef PX4_SPIDEV_MPU - { MPU9250_BUS_SPI_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU, nullptr }, + { MPU9250_BUS_SPI_INTERNAL, MPU_DEVICE_PATH, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU, nullptr }, #endif #ifdef PX4_SPIDEV_MPU2 - { MPU9250_BUS_SPI_INTERNAL2, MPU_DEVICE_PATH_ACCEL_1, MPU_DEVICE_PATH_GYRO_1, MPU_DEVICE_PATH_MAG_1, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU2, nullptr }, + { MPU9250_BUS_SPI_INTERNAL2, MPU_DEVICE_PATH_1, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU2, nullptr }, #endif #if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_MPU) - { MPU9250_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT, &MPU9250_SPI_interface, true, PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, nullptr }, + { MPU9250_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_EXT, &MPU9250_SPI_interface, true, PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, nullptr }, #endif }; @@ -158,7 +124,6 @@ void start(enum MPU9250_BUS busid, enum Rotation rotation, bool external_bus, bo bool start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external_bus, bool magnetometer_only); struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid); void stop(enum MPU9250_BUS busid); -void reset(enum MPU9250_BUS busid); void info(enum MPU9250_BUS busid); void usage(); @@ -183,8 +148,6 @@ struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid) bool start_bus(struct mpu9250_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) { @@ -217,8 +180,7 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external, #endif - bus.dev = new MPU9250(interface, mag_interface, bus.accelpath, bus.gyropath, bus.magpath, rotation, - magnetometer_only); + bus.dev = new MPU9250(interface, mag_interface, bus.path, rotation, magnetometer_only); if (bus.dev == nullptr) { delete interface; @@ -234,38 +196,10 @@ start_bus(struct mpu9250_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. - */ - if (bus.dev->get_whoami() == MPU_WHOAMI_6500) { - fd = open(bus.accelpath, O_RDONLY); - - } else { - 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; @@ -324,32 +258,6 @@ stop(enum MPU9250_BUS busid) exit(0); } -/** - * Reset the driver. - */ -void -reset(enum MPU9250_BUS busid) -{ - struct mpu9250_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. */ @@ -372,7 +280,7 @@ info(enum MPU9250_BUS busid) void usage() { - PX4_INFO("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'testerror'"); + PX4_INFO("missing command: try 'start', 'info', 'test', 'stop',\n 'regdump', 'testerror'"); PX4_INFO("options:"); PX4_INFO(" -X (i2c external bus)"); PX4_INFO(" -I (i2c internal bus)"); @@ -451,13 +359,6 @@ mpu9250_main(int argc, char *argv[]) mpu9250::stop(busid); } - /* - * Reset the driver. - */ - if (!strcmp(verb, "reset")) { - mpu9250::reset(busid); - } - /* * Print driver information. */ diff --git a/src/drivers/imu/mpu9250/mpu9250_spi.cpp b/src/drivers/imu/mpu9250/mpu9250_spi.cpp index c02d6f2dc4c6..fa429aa3b07f 100644 --- a/src/drivers/imu/mpu9250/mpu9250_spi.cpp +++ b/src/drivers/imu/mpu9250/mpu9250_spi.cpp @@ -43,7 +43,6 @@ #include #include -#include #include #include "mpu9250.h" @@ -120,7 +119,7 @@ MPU9250_SPI::set_bus_frequency(unsigned ®_speed) int MPU9250_SPI::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; @@ -144,7 +143,7 @@ MPU9250_SPI::read(unsigned reg_speed, void *data, unsigned count) * 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 cmd[3] {}; uint8_t *pbuff = count < sizeof(MPUReport) ? cmd : (uint8_t *) data ; diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp index d6d33e0575d6..6aebd05d2595 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp @@ -104,8 +104,10 @@ void PX4Magnetometer::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_ float zraw_f = z; rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + const matrix::Vector3f raw_f{xraw_f, yraw_f, zraw_f}; + // Apply range scale and the calibrating offset/scale - const matrix::Vector3f val_calibrated{(((matrix::Vector3f{xraw_f, yraw_f, zraw_f} * report.scaling) - _calibration_offset).emult(_calibration_scale))}; + const matrix::Vector3f val_calibrated{(((raw_f.emult(_sensitivity) * report.scaling) - _calibration_offset).emult(_calibration_scale))}; // Raw values (ADC units 0 - 65535) report.x_raw = x; diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp index 00f004965cc0..4116c9960a67 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp @@ -53,6 +53,7 @@ class PX4Magnetometer : public cdev::CDev void set_scale(float scale) { _sensor_mag_pub.get().scaling = scale; } void set_temperature(float temperature) { _sensor_mag_pub.get().temperature = temperature; } void set_external(bool external) { _sensor_mag_pub.get().is_external = external; } + void set_sensitivity(float x, float y, float z) { _sensitivity = matrix::Vector3f{x, y, z}; } void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z); @@ -67,6 +68,8 @@ class PX4Magnetometer : public cdev::CDev matrix::Vector3f _calibration_scale{1.0f, 1.0f, 1.0f}; matrix::Vector3f _calibration_offset{0.0f, 0.0f, 0.0f}; + matrix::Vector3f _sensitivity{1.0f, 1.0f, 1.0f}; + int _class_device_instance{-1}; };