mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-20 11:39:07 +08:00
InvenSense MPU9250 move to PX4Accelerometer and PX4Gyroscope helpers
This commit is contained in:
Daniel Agar
parent
cd45d8fc68
commit
32fb2bae8c
@ -39,11 +39,10 @@
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <drivers/device/i2c.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_device.h>
|
||||
|
||||
#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
|
||||
@ -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
|
||||
)
|
||||
|
||||
@ -45,65 +45,29 @@
|
||||
#include <px4_time.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/device/spi.h>
|
||||
#include <drivers/device/ringbuffer.h>
|
||||
#include <drivers/device/integrator.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
|
||||
#include <lib/conversion/rotation.h>
|
||||
|
||||
#include "mag.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;
|
||||
}
|
||||
@ -33,20 +33,18 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "drivers/device/ringbuffer.h" // ringbuffer::RingBuffer
|
||||
#include "drivers/drv_mag.h" // mag_calibration_s
|
||||
#include <cdev/CDev.hpp>
|
||||
#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 */
|
||||
|
||||
#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 @@ 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 MPU9250;
|
||||
|
||||
@ -157,7 +154,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);
|
||||
@ -165,27 +162,23 @@ protected:
|
||||
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 &);
|
||||
@ -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 <px4_config.h>
|
||||
#include <ecl/geo/geo.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <errno.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <perf/perf_counter.h>
|
||||
|
||||
#include <board_config.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <drivers/device/spi.h>
|
||||
#include <drivers/device/ringbuffer.h>
|
||||
#include <drivers/device/integrator.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <mathlib/math/filter/LowPassFilter2p.hpp>
|
||||
#include <lib/conversion/rotation.h>
|
||||
|
||||
#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);
|
||||
}
|
||||
}
|
||||
@ -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};
|
||||
|
||||
};
|
||||
@ -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 <px4_config.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <drivers/device/spi.h>
|
||||
#include <drivers/device/ringbuffer.h>
|
||||
#include <drivers/device/integrator.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
|
||||
#include <lib/conversion/rotation.h>
|
||||
|
||||
#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);
|
||||
}
|
||||
}
|
||||
@ -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};
|
||||
};
|
||||
@ -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 <systemlib/px4_macros.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/device/spi.h>
|
||||
#include <drivers/device/ringbuffer.h>
|
||||
#include <drivers/device/integrator.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
|
||||
#include <lib/conversion/rotation.h>
|
||||
|
||||
#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();
|
||||
}
|
||||
|
||||
@ -35,26 +35,14 @@
|
||||
|
||||
#include <perf/perf_counter.h>
|
||||
#include <systemlib/conversions.h>
|
||||
|
||||
#include <board_config.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <drivers/device/ringbuffer.h>
|
||||
#include <drivers/device/integrator.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <mathlib/math/filter/LowPassFilter2p.hpp>
|
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
|
||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||
#include <lib/conversion/rotation.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
#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 @@ public:
|
||||
|
||||
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 @@ 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 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 @@ private:
|
||||
*/
|
||||
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
|
||||
*/
|
||||
|
||||
@ -39,7 +39,6 @@
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <drivers/device/i2c.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_device.h>
|
||||
|
||||
#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;
|
||||
|
||||
@ -43,53 +43,21 @@
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <stdio.h>
|
||||
#include <px4_getopt.h>
|
||||
|
||||
#include <perf/perf_counter.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/conversions.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 <drivers/device/ringbuffer.h>
|
||||
#include <drivers/device/integrator.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <mathlib/math/filter/LowPassFilter2p.hpp>
|
||||
#include <lib/conversion/rotation.h>
|
||||
|
||||
#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.
|
||||
*/
|
||||
@ -43,7 +43,6 @@
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <drivers/device/spi.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_device.h>
|
||||
|
||||
#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 ;
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -55,6 +55,7 @@ public:
|
||||
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);
|
||||
|
||||
@ -69,6 +70,8 @@ private:
|
||||
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};
|
||||
|
||||
};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user