fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 05:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

InvenSense ICM20948 move to PX4Accelerometer, PX4Gyroscope, PX4Magnetometer helpers

Browse Source
This commit is contained in:
Daniel Agar
2019-06-01 11:43:16 -04:00
parent 5421ef5535
commit 6e781c2289
13 changed files with 223 additions and 1251 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/drv_sensor.h
+1
View File
@@ -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
src/drivers/imu/icm20948/CMakeLists.txt
+2 -4
View File
@@ -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
)
src/drivers/imu/icm20948/mag.cpp → src/drivers/imu/icm20948/ICM20948_mag.cpp
+28 -196
View File
@@ -40,56 +40,34 @@
*
*/
#include "mag.h"
#include "icm20948.h"
#include <px4_config.h>
#include <px4_log.h>
#include <px4_time.h>
#include <lib/perf/perf_counter.h>
#include <drivers/drv_hrt.h>
#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;
}
src/drivers/imu/icm20948/mag.h → src/drivers/imu/icm20948/ICM20948_mag.h
+17 -26
View File
@@ -33,16 +33,12 @@
#pragma once
#include <drivers/device/i2c.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_device.h>
#include <drivers/drv_mag.h>
#include <lib/perf/perf_counter.h>
#include <uORB/uORB.h>
#include <perf/perf_counter.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <drivers/device/Device.hpp>
/* 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 &);
src/drivers/imu/icm20948/accel.cpp
-123
View File
@@ -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 <drivers/device/spi.h>
#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);
}
}
src/drivers/imu/icm20948/accel.h
-67
View File
@@ -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 <drivers/device/device.h>
#include <drivers/drv_accel.h>
#include <uORB/uORB.h>
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};
};
src/drivers/imu/icm20948/gyro.cpp
-100
View File
@@ -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);
}
}
src/drivers/imu/icm20948/gyro.h
-66
View File
@@ -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 <drivers/device/device.h>
#include <drivers/drv_gyro.h>
#include <uORB/uORB.h>
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};
};
src/drivers/imu/icm20948/icm20948.cpp
+121 -526
View File
@@ -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 <px4_config.h>
#include <px4_time.h>
#include <lib/ecl/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <systemlib/conversions.h>
#include <systemlib/px4_macros.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/spi.h>
#include <lib/conversion/rotation.h>
#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();
}
src/drivers/imu/icm20948/icm20948.h
+32 -70
View File
@@ -31,27 +31,24 @@
*
****************************************************************************/
#include <board_config.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/device/integrator.h>
#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/conversion/rotation.h>
#include <lib/ecl/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <px4_config.h>
#include <px4_getopt.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#include <sys/types.h>
#pragma once
#include <stdint.h>
#include <perf/perf_counter.h>
#include <systemlib/conversions.h>
#include <drivers/drv_hrt.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/conversion/rotation.h>
#include <systemlib/err.h>
#include <systemlib/px4_macros.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/uORB.h>
#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 &);
};
src/drivers/imu/icm20948/icm20948_i2c.cpp
+2
View File
@@ -37,7 +37,9 @@
* I2C interface for ICM20948
*/
#include <px4_config.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_device.h>
#include "icm20948.h"
src/drivers/imu/icm20948/main.cpp → src/drivers/imu/icm20948/icm20948_main.cpp
+15 -70
View File
@@ -39,11 +39,18 @@
* based on the mpu9250 driver
*/
#include <px4_config.h>
#include <px4_getopt.h>
#include <lib/perf/perf_counter.h>
#include <lib/systemlib/conversions.h>
#include <board_config.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/spi.h>
#include <lib/conversion/rotation.h>
#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.
*/
src/drivers/imu/icm20948/mag_i2c.cpp
+5 -3
View File
@@ -38,7 +38,9 @@
*/
#include "icm20948.h"
#include "mag.h"
#include "ICM20948_mag.h"
#include <drivers/device/i2c.h>
#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