Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 06:14:14 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

InvenSense MPU9250 move to PX4Accelerometer and PX4Gyroscope helpers

Browse Source
This commit is contained in:
Daniel Agar
2019-05-24 13:54:22 -04:00
parent cd45d8fc68
commit 32fb2bae8c
15 changed files with 134 additions and 1214 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/mpu9250/mag_i2c.cpp → src/drivers/imu/mpu9250/AK8963_I2C.cpp
+3 -5
View File
@@ -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
src/drivers/imu/mpu9250/CMakeLists.txt
+6 -5
View File
@@ -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
)
src/drivers/imu/mpu9250/mag.cpp → src/drivers/imu/mpu9250/MPU9250_mag.cpp
+17 -172
View File
@@ -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;
}
src/drivers/imu/mpu9250/mag.h → src/drivers/imu/mpu9250/MPU9250_mag.h
+17 -24
View File
@@ -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 &);
src/drivers/imu/mpu9250/accel.cpp
-147
View File
@@ -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);
}
}
src/drivers/imu/mpu9250/accel.h
-63
View File
@@ -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};
};
src/drivers/imu/mpu9250/gyro.cpp
-113
View File
@@ -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);
}
}
src/drivers/imu/mpu9250/gyro.h
-62
View File
@@ -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};
};
src/drivers/imu/mpu9250/mpu9250.cpp
+41 -428
View File
File diff suppressed because it is too large Load Diff
src/drivers/imu/mpu9250/mpu9250.h
+24 -73
View File
@@ -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
*/
src/drivers/imu/mpu9250/mpu9250_i2c.cpp
+1 -2
View File
@@ -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;
src/drivers/imu/mpu9250/main.cpp → src/drivers/imu/mpu9250/mpu9250_main.cpp
+17 -116
View File
@@ -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.
*/
src/drivers/imu/mpu9250/mpu9250_spi.cpp
+2 -3
View File
@@ -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 &reg_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 ;
src/lib/drivers/magnetometer/PX4Magnetometer.cpp
+3 -1
View File
@@ -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;
src/lib/drivers/magnetometer/PX4Magnetometer.hpp
+3
View File
@@ -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};
};