fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 20:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

organize all IMU drivers in subdirectory

Browse Source
This commit is contained in:
Daniel Agar
2018-02-09 22:40:34 +01:00
committed by Lorenz Meier
parent f7a17ad5e5
commit 681e351f62
54 changed files with 56 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/adis16448/CMakeLists.txt
+43
View File
@@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2015 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.
#
############################################################################
px4_add_module(
MODULE drivers__adis16448
MAIN adis16448
STACK_MAIN 1200
COMPILE_FLAGS
SRCS
adis16448.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/drivers/imu/adis16448/adis16448.cpp
+2054
View File
File diff suppressed because it is too large Load Diff
src/drivers/imu/bma180/CMakeLists.txt
+42
View File
@@ -0,0 +1,42 @@
############################################################################
#
# Copyright (c) 2015 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.
#
############################################################################
px4_add_module(
MODULE drivers__bma180
MAIN bma180
COMPILE_FLAGS
SRCS
bma180.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/drivers/imu/bma180/bma180.cpp
+933
View File
@@ -0,0 +1,933 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 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 bma180.cpp
* Driver for the Bosch BMA 180 MEMS accelerometer connected via SPI.
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdlib.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <drivers/drv_hrt.h>
#include <board_config.h>
#include <drivers/device/spi.h>
#include <drivers/drv_accel.h>
#include <drivers/device/ringbuffer.h>
#define ACCEL_DEVICE_PATH "/dev/bma180"
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
#define ADDR_CHIP_ID 0x00
#define CHIP_ID 0x03
#define ADDR_ACC_X_LSB 0x02
#define ADDR_ACC_Y_LSB 0x04
#define ADDR_ACC_Z_LSB 0x06
#define ADDR_TEMPERATURE 0x08
#define ADDR_CTRL_REG0 0x0D
#define REG0_WRITE_ENABLE 0x10
#define ADDR_RESET 0x10
#define SOFT_RESET 0xB6
#define ADDR_BW_TCS 0x20
#define BW_TCS_BW_MASK (0xf<<4)
#define BW_TCS_BW_10HZ (0<<4)
#define BW_TCS_BW_20HZ (1<<4)
#define BW_TCS_BW_40HZ (2<<4)
#define BW_TCS_BW_75HZ (3<<4)
#define BW_TCS_BW_150HZ (4<<4)
#define BW_TCS_BW_300HZ (5<<4)
#define BW_TCS_BW_600HZ (6<<4)
#define BW_TCS_BW_1200HZ (7<<4)
#define ADDR_HIGH_DUR 0x27
#define HIGH_DUR_DIS_I2C (1<<0)
#define ADDR_TCO_Z 0x30
#define TCO_Z_MODE_MASK 0x3
#define ADDR_GAIN_Y 0x33
#define GAIN_Y_SHADOW_DIS (1<<0)
#define ADDR_OFFSET_LSB1 0x35
#define OFFSET_LSB1_RANGE_MASK (7<<1)
#define OFFSET_LSB1_RANGE_1G (0<<1)
#define OFFSET_LSB1_RANGE_2G (2<<1)
#define OFFSET_LSB1_RANGE_3G (3<<1)
#define OFFSET_LSB1_RANGE_4G (4<<1)
#define OFFSET_LSB1_RANGE_8G (5<<1)
#define OFFSET_LSB1_RANGE_16G (6<<1)
#define ADDR_OFFSET_T 0x37
#define OFFSET_T_READOUT_12BIT (1<<0)
extern "C" { __EXPORT int bma180_main(int argc, char *argv[]); }
class BMA180 : public device::SPI
{
public:
BMA180(int bus, uint32_t device);
virtual ~BMA180();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
private:
struct hrt_call _call;
unsigned _call_interval;
ringbuffer::RingBuffer *_reports;
struct accel_calibration_s _accel_scale;
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
int _class_instance;
unsigned _current_lowpass;
unsigned _current_range;
perf_counter_t _sample_perf;
/**
* Start automatic measurement.
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
*
* Called by the HRT in interrupt context at the specified rate if
* automatic polling is enabled.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void measure_trampoline(void *arg);
/**
* Fetch measurements from the sensor and update the report ring.
*/
void measure();
/**
* Read a register from the BMA180
*
* @param The register to read.
* @return The value that was read.
*/
uint8_t read_reg(unsigned reg);
/**
* Write a register in the BMA180
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_reg(unsigned reg, uint8_t value);
/**
* Modify a register in the BMA180
*
* Bits are cleared before bits are set.
*
* @param reg The register to modify.
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Set the BMA180 measurement range.
*
* @param max_g The maximum G value the range must support.
* @return OK if the value can be supported, -ERANGE otherwise.
*/
int set_range(unsigned max_g);
/**
* Set the BMA180 internal lowpass filter frequency.
*
* @param frequency The internal lowpass filter frequency is set to a value
* equal or greater to this.
* Zero selects the highest frequency supported.
* @return OK if the value can be supported.
*/
int set_lowpass(unsigned frequency);
};
BMA180::BMA180(int bus, uint32_t device) :
SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000),
_call_interval(0),
_reports(nullptr),
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(nullptr),
_class_instance(-1),
_current_lowpass(0),
_current_range(0),
_sample_perf(perf_alloc(PC_ELAPSED, "bma180_read"))
{
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_BMA180;
// default 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;
}
BMA180::~BMA180()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_reports != nullptr) {
delete _reports;
}
/* delete the perf counter */
perf_free(_sample_perf);
}
int
BMA180::init()
{
int ret = PX4_ERROR;
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_reports == nullptr) {
goto out;
}
/* perform soft reset (p48) */
write_reg(ADDR_RESET, SOFT_RESET);
/* wait 10 ms (datasheet incorrectly lists 10 us on page 49) */
usleep(10000);
/* enable writing to chip config */
modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
/* disable I2C interface */
modify_reg(ADDR_HIGH_DUR, HIGH_DUR_DIS_I2C, 0);
/* switch to low-noise mode */
modify_reg(ADDR_TCO_Z, TCO_Z_MODE_MASK, 0);
/* disable 12-bit mode */
modify_reg(ADDR_OFFSET_T, OFFSET_T_READOUT_12BIT, 0);
/* disable shadow-disable mode */
modify_reg(ADDR_GAIN_Y, GAIN_Y_SHADOW_DIS, 0);
/* disable writing to chip config */
modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
if (set_range(4)) { warnx("Failed setting range"); }
if (set_lowpass(75)) { warnx("Failed setting lowpass"); }
if (read_reg(ADDR_CHIP_ID) == CHIP_ID) {
ret = OK;
} else {
ret = PX4_ERROR;
}
_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
/* advertise sensor topic, measure manually to initialize valid report */
measure();
if (_class_instance == CLASS_DEVICE_PRIMARY) {
struct accel_report arp;
_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
}
out:
return ret;
}
int
BMA180::probe()
{
/* dummy read to ensure SPI state machine is sane */
read_reg(ADDR_CHIP_ID);
if (read_reg(ADDR_CHIP_ID) == CHIP_ID) {
return OK;
}
return -EIO;
}
ssize_t
BMA180::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct accel_report);
struct accel_report *arp = reinterpret_cast<struct accel_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_call_interval > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the measurement code while we are doing this;
* we are careful to avoid racing with it.
*/
while (count--) {
if (_reports->get(arp)) {
ret += sizeof(*arp);
arp++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement */
_reports->flush();
measure();
/* measurement will have generated a report, copy it out */
if (_reports->get(arp)) {
ret = sizeof(*arp);
}
return ret;
}
int
BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_call_interval = 0;
return OK;
/* external signalling not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT:
/* With internal low pass filters enabled, 250 Hz is sufficient */
return ioctl(filp, SENSORIOCSPOLLRATE, 250);
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_call_interval == 0);
/* convert hz to hrt interval via microseconds */
unsigned ticks = 1000000 / arg;
/* check against maximum sane rate */
if (ticks < 1000) {
return -EINVAL;
}
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_call.period = _call_interval = ticks;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_call_interval == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return 1000000 / _call_interval;
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = px4_enter_critical_section();
if (!_reports->resize(arg)) {
px4_leave_critical_section(flags);
return -ENOMEM;
}
px4_leave_critical_section(flags);
return OK;
}
case SENSORIOCRESET:
/* XXX implement */
return -EINVAL;
case ACCELIOCSSAMPLERATE: /* sensor sample rate is not (really) adjustable */
return -EINVAL;
case ACCELIOCGSAMPLERATE:
return 1200; /* always operating in low-noise mode */
case ACCELIOCSSCALE:
/* copy scale in */
memcpy(&_accel_scale, (struct accel_calibration_s *) arg, sizeof(_accel_scale));
return OK;
case ACCELIOCGSCALE:
/* copy scale out */
memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale));
return OK;
case ACCELIOCSRANGE:
return set_range(arg);
case ACCELIOCGRANGE:
return _current_range;
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
}
}
uint8_t
BMA180::read_reg(unsigned reg)
{
uint8_t cmd[2];
cmd[0] = reg | DIR_READ;
transfer(cmd, cmd, sizeof(cmd));
return cmd[1];
}
void
BMA180::write_reg(unsigned reg, uint8_t value)
{
uint8_t cmd[2];
cmd[0] = reg | DIR_WRITE;
cmd[1] = value;
transfer(cmd, nullptr, sizeof(cmd));
}
void
BMA180::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
{
uint8_t val;
val = read_reg(reg);
val &= ~clearbits;
val |= setbits;
write_reg(reg, val);
}
int
BMA180::set_range(unsigned max_g)
{
uint8_t rangebits;
if (max_g == 0) {
max_g = 16;
}
if (max_g > 16) {
return -ERANGE;
}
if (max_g <= 2) {
_current_range = 2;
rangebits = OFFSET_LSB1_RANGE_2G;
} else if (max_g <= 3) {
_current_range = 3;
rangebits = OFFSET_LSB1_RANGE_3G;
} else if (max_g <= 4) {
_current_range = 4;
rangebits = OFFSET_LSB1_RANGE_4G;
} else if (max_g <= 8) {
_current_range = 8;
rangebits = OFFSET_LSB1_RANGE_8G;
} else if (max_g <= 16) {
_current_range = 16;
rangebits = OFFSET_LSB1_RANGE_16G;
} else {
return -EINVAL;
}
/* set new range scaling factor */
_accel_range_m_s2 = _current_range * 9.80665f;
_accel_range_scale = _accel_range_m_s2 / 8192.0f;
/* enable writing to chip config */
modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
/* adjust sensor configuration */
modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits);
/* block writing to chip config */
modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
/* check if wanted value is now in register */
return !((read_reg(ADDR_OFFSET_LSB1) & OFFSET_LSB1_RANGE_MASK) ==
(OFFSET_LSB1_RANGE_MASK & rangebits));
}
int
BMA180::set_lowpass(unsigned frequency)
{
uint8_t bwbits;
if (frequency > 1200) {
return -ERANGE;
} else if (frequency > 600) {
bwbits = BW_TCS_BW_1200HZ;
} else if (frequency > 300) {
bwbits = BW_TCS_BW_600HZ;
} else if (frequency > 150) {
bwbits = BW_TCS_BW_300HZ;
} else if (frequency > 75) {
bwbits = BW_TCS_BW_150HZ;
} else if (frequency > 40) {
bwbits = BW_TCS_BW_75HZ;
} else if (frequency > 20) {
bwbits = BW_TCS_BW_40HZ;
} else if (frequency > 10) {
bwbits = BW_TCS_BW_20HZ;
} else {
bwbits = BW_TCS_BW_10HZ;
}
/* enable writing to chip config */
modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
/* adjust sensor configuration */
modify_reg(ADDR_BW_TCS, BW_TCS_BW_MASK, bwbits);
/* block writing to chip config */
modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
/* check if wanted value is now in register */
return !((read_reg(ADDR_BW_TCS) & BW_TCS_BW_MASK) ==
(BW_TCS_BW_MASK & bwbits));
}
void
BMA180::start()
{
/* make sure we are stopped first */
stop();
/* reset the report ring */
_reports->flush();
/* start polling at the specified rate */
hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this);
}
void
BMA180::stop()
{
hrt_cancel(&_call);
}
void
BMA180::measure_trampoline(void *arg)
{
BMA180 *dev = (BMA180 *)arg;
/* make another measurement */
dev->measure();
}
void
BMA180::measure()
{
/* BMA180 measurement registers */
// #pragma pack(push, 1)
// struct {
// uint8_t cmd;
// int16_t x;
// int16_t y;
// int16_t z;
// } raw_report;
// #pragma pack(pop)
struct accel_report report;
/* start the performance counter */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the BMA180 in one pass;
* starting from the X LSB.
*/
//raw_report.cmd = ADDR_ACC_X_LSB;
// XXX PX4DEV transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
/*
* Adjust and scale results to SI units.
*
* Note that we ignore the "new data" bits. At any time we read, each
* of the axis measurements are the "most recent", even if we've seen
* them before. There is no good way to synchronise with the internal
* measurement flow without using the external interrupt.
*/
report.timestamp = hrt_absolute_time();
report.error_count = 0;
/*
* y of board is x of sensor and x of board is -y of sensor
* perform only the axis assignment here.
* Two non-value bits are discarded directly
*/
report.y_raw = read_reg(ADDR_ACC_X_LSB + 0);
report.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
report.x_raw = read_reg(ADDR_ACC_X_LSB + 2);
report.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
report.z_raw = read_reg(ADDR_ACC_X_LSB + 4);
report.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
/* discard two non-value bits in the 16 bit measurement */
report.x_raw = (report.x_raw / 4);
report.y_raw = (report.y_raw / 4);
report.z_raw = (report.z_raw / 4);
/* invert y axis, due to 14 bit data no overflow can occur in the negation */
report.y_raw = -report.y_raw;
report.x = ((report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
report.y = ((report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
report.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
report.scaling = _accel_range_scale;
report.range_m_s2 = _accel_range_m_s2;
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
if (_accel_topic != nullptr && !(_pub_blocked)) {
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
}
/* stop the perf counter */
perf_end(_sample_perf);
}
void
BMA180::print_info()
{
perf_print_counter(_sample_perf);
_reports->print_info("report queue");
}
/**
* Local functions in support of the shell command.
*/
namespace bma180
{
BMA180 *g_dev;
void start();
void test();
void reset();
void info();
/**
* Start the driver.
*/
void
start()
{
int fd;
if (g_dev != nullptr) {
errx(1, "already started");
}
/* create the driver */
g_dev = new BMA180(1 /* XXX magic number */, PX4_SPIDEV_BMA);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
int fd = -1;
struct accel_report a_report;
ssize_t sz;
/* get the driver */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'bma180 start' if the driver is not running)",
ACCEL_DEVICE_PATH);
/* reset to manual polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) {
err(1, "reset to manual polling");
}
/* do a simple demand read */
sz = read(fd, &a_report, sizeof(a_report));
if (sz != sizeof(a_report)) {
err(1, "immediate acc read failed");
}
warnx("single read");
warnx("time: %lld", a_report.timestamp);
warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x);
warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y);
warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z);
warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw);
warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
(double)(a_report.range_m_s2 / 9.81f));
/* XXX add poll-rate tests here too */
reset();
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = open(ACCEL_DEVICE_PATH, 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");
}
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr) {
errx(1, "BMA180: driver not running");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
} // namespace
int
bma180_main(int argc, char *argv[])
{
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
bma180::start();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test")) {
bma180::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset")) {
bma180::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info")) {
bma180::info();
}
errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
}
src/drivers/imu/bmi055/CMakeLists.txt
+45
View File
@@ -0,0 +1,45 @@
############################################################################
#
# Copyright (c) 2015 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.
#
############################################################################
px4_add_module(
MODULE drivers__bmi055
MAIN bmi055
STACK_MAIN 1200
COMPILE_FLAGS
SRCS
bmi055_accel.cpp
bmi055_gyro.cpp
bmi055_main.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/drivers/imu/bmi055/bmi055.hpp
+638
View File
@@ -0,0 +1,638 @@
#ifndef BMI055_HPP_
#define BMI055_HPP_
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdlib.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <getopt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/conversions.h>
#include <nuttx/arch.h>
#include <nuttx/clock.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>
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#define BMI055_DEVICE_PATH_ACCEL "/dev/bmi055_accel"
#define BMI055_DEVICE_PATH_GYRO "/dev/bmi055_gyro"
#define BMI055_DEVICE_PATH_ACCEL_EXT "/dev/bmi055_accel_ext"
#define BMI055_DEVICE_PATH_GYRO_EXT "/dev/bmi055_gyro_ext"
// BMI055 Accel registers
#define BMI055_ACC_CHIP_ID 0x00
#define BMI055_ACC_X_L 0x02
#define BMI055_ACC_X_H 0x03
#define BMI055_ACC_Y_L 0x04
#define BMI055_ACC_Y_H 0x05
#define BMI055_ACC_Z_L 0x06
#define BMI055_ACC_Z_H 0x07
#define BMI055_ACC_TEMP 0x08
#define BMI055_ACC_INT_STATUS_0 0x09
#define BMI055_ACC_INT_STATUS_1 0x0A
#define BMI055_ACC_INT_STATUS_2 0x0B
#define BMI055_ACC_INT_STATUS_3 0x0C
#define BMI055_ACC_FIFO_STATUS 0x0E
#define BMI055_ACC_RANGE 0x0F
#define BMI055_ACC_BW 0x10
#define BMI055_ACC_PMU_LPW 0x11
#define BMI055_ACC_PMU_LOW_POWER 0x12
#define BMI055_ACC_DATA_CTRL 0x13
#define BMI055_ACC_SOFTRESET 0x14
#define BMI055_ACC_INT_EN_0 0x16
#define BMI055_ACC_INT_EN_1 0x17
#define BMI055_ACC_INT_EN_2 0x18
#define BMI055_ACC_INT_MAP_0 0x19
#define BMI055_ACC_INT_MAP_1 0x1A
#define BMI055_ACC_INT_MAP_2 0x1B
#define BMI055_ACC_INT_SRC 0x1E
#define BMI055_ACC_INT_OUT_CTRL 0x20
#define BMI055_ACC_INT_LATCH 0x21
#define BMI055_ACC_INT_LH_0 0x22
#define BMI055_ACC_INT_LH_1 0x23
#define BMI055_ACC_INT_LH_2 0x24
#define BMI055_ACC_INT_LH_3 0x25
#define BMI055_ACC_INT_LH_4 0x26
#define BMI055_ACC_INT_MOT_0 0x27
#define BMI055_ACC_INT_MOT_1 0x28
#define BMI055_ACC_INT_MOT_2 0x29
#define BMI055_ACC_INT_TAP_0 0x2A
#define BMI055_ACC_INT_TAP_1 0x2B
#define BMI055_ACC_INT_ORIE_0 0x2C
#define BMI055_ACC_INT_ORIE_1 0x2D
#define BMI055_ACC_INT_FLAT_0 0x2E
#define BMI055_ACC_INT_FLAT_1 0x2F
#define BMI055_ACC_FIFO_CONFIG_0 0x30
#define BMI055_ACC_SELF_TEST 0x32
#define BMI055_ACC_EEPROM_CTRL 0x33
#define BMI055_ACC_SERIAL_CTRL 0x34
#define BMI055_ACC_OFFSET_CTRL 0x36
#define BMI055_ACC_OFC_SETTING 0x37
#define BMI055_ACC_OFFSET_X 0x38
#define BMI055_ACC_OFFSET_Y 0x39
#define BMI055_ACC_OFFSET_Z 0x3A
#define BMI055_ACC_TRIM_GPO 0x3B
#define BMI055_ACC_TRIM_GP1 0x3C
#define BMI055_ACC_FIFO_CONFIG_1 0x3E
#define BMI055_ACC_FIFO_DATA 0x3F
// BMI055 Gyro registers
#define BMI055_GYR_CHIP_ID 0x00
#define BMI055_GYR_X_L 0x02
#define BMI055_GYR_X_H 0x03
#define BMI055_GYR_Y_L 0x04
#define BMI055_GYR_Y_H 0x05
#define BMI055_GYR_Z_L 0x06
#define BMI055_GYR_Z_H 0x07
#define BMI055_GYR_INT_STATUS_0 0x09
#define BMI055_GYR_INT_STATUS_1 0x0A
#define BMI055_GYR_INT_STATUS_2 0x0B
#define BMI055_GYR_INT_STATUS_3 0x0C
#define BMI055_GYR_FIFO_STATUS 0x0E
#define BMI055_GYR_RANGE 0x0F
#define BMI055_GYR_BW 0x10
#define BMI055_GYR_LPM1 0x11
#define BMI055_GYR_LPM2 0x12
#define BMI055_GYR_RATE_HBW 0x13
#define BMI055_GYR_SOFTRESET 0x14
#define BMI055_GYR_INT_EN_0 0x15
#define BMI055_GYR_INT_EN_1 0x16
#define BMI055_GYR_INT_MAP_0 0x17
#define BMI055_GYR_INT_MAP_1 0x18
#define BMI055_GYR_INT_MAP_2 0x19
#define BMI055_GYRO_0_REG 0x1A
#define BMI055_GYRO_1_REG 0x1B
#define BMI055_GYRO_2_REG 0x1C
#define BMI055_GYRO_3_REG 0x1E
#define BMI055_GYR_INT_LATCH 0x21
#define BMI055_GYR_INT_LH_0 0x22
#define BMI055_GYR_INT_LH_1 0x23
#define BMI055_GYR_INT_LH_2 0x24
#define BMI055_GYR_INT_LH_3 0x25
#define BMI055_GYR_INT_LH_4 0x26
#define BMI055_GYR_INT_LH_5 0x27
#define BMI055_GYR_SOC 0x31
#define BMI055_GYR_A_FOC 0x32
#define BMI055_GYR_TRIM_NVM_CTRL 0x33
#define BMI055_BGW_SPI3_WDT 0x34
#define BMI055_GYR_OFFSET_COMP 0x36
#define BMI055_GYR_OFFSET_COMP_X 0x37
#define BMI055_GYR_OFFSET_COMP_Y 0x38
#define BMI055_GYR_OFFSET_COMP_Z 0x39
#define BMI055_GYR_TRIM_GPO 0x3A
#define BMI055_GYR_TRIM_GP1 0x3B
#define BMI055_GYR_SELF_TEST 0x3C
#define BMI055_GYR_FIFO_CONFIG_0 0x3D
#define BMI055_GYR_FIFO_CONFIG_1 0x3E
#define BMI055_GYR_FIFO_DATA 0x3F
// BMI055 Accelerometer Chip-Id
#define BMI055_ACC_WHO_AM_I 0xFA
// BMI055 Gyroscope Chip-Id
#define BMI055_GYR_WHO_AM_I 0x0F
//BMI055_ACC_BW 0x10
#define BMI055_ACCEL_BW_7_81 (1<<3) | (0<<2) | (0<<1) | (0<<0)
#define BMI055_ACCEL_BW_15_63 (1<<3) | (0<<2) | (0<<1) | (1<<0)
#define BMI055_ACCEL_BW_31_25 (1<<3) | (0<<2) | (1<<1) | (0<<0)
#define BMI055_ACCEL_BW_62_5 (1<<3) | (0<<2) | (1<<1) | (1<<0)
#define BMI055_ACCEL_BW_125 (1<<3) | (1<<2) | (0<<1) | (0<<0)
#define BMI055_ACCEL_BW_250 (1<<3) | (1<<2) | (0<<1) | (1<<0)
#define BMI055_ACCEL_BW_500 (1<<3) | (1<<2) | (1<<1) | (0<<0)
#define BMI055_ACCEL_BW_1000 (1<<3) | (1<<2) | (1<<1) | (1<<0)
//BMI055_ACC_PMU_LPW 0x11
#define BMI055_ACCEL_NORMAL (0<<7) | (0<<6) | (0<<5)
#define BMI055_ACCEL_DEEP_SUSPEND (0<<7) | (0<<6) | (1<<5)
#define BMI055_ACCEL_LOW_POWER (0<<7) | (1<<6) | (0<<5)
#define BMI055_ACCEL_SUSPEND (1<<7) | (0<<6) | (0<<5)
//BMI055_ACC_RANGE 0x0F
#define BMI055_ACCEL_RANGE_2_G (0<<3) | (0<<2) | (1<<1) | (1<<0)
#define BMI055_ACCEL_RANGE_4_G (0<<3) | (1<<2) | (0<<1) | (1<<0)
#define BMI055_ACCEL_RANGE_8_G (1<<3) | (0<<2) | (0<<1) | (0<<0)
#define BMI055_ACCEL_RANGE_16_G (1<<3) | (1<<2) | (0<<1) | (0<<0)
//BMI055_GYR_BW 0x10
#define BMI055_GYRO_RATE_100 (0<<3) | (1<<2) | (1<<1) | (1<<0)
#define BMI055_GYRO_RATE_200 (0<<3) | (1<<2) | (1<<1) | (0<<0)
#define BMI055_GYRO_RATE_400 (0<<3) | (0<<2) | (1<<1) | (1<<0)
#define BMI055_GYRO_RATE_1000 (0<<3) | (0<<2) | (1<<1) | (0<<0)
#define BMI055_GYRO_RATE_2000 (0<<3) | (0<<2) | (0<<1) | (1<<0)
//BMI055_GYR_LPM1 0x11
#define BMI055_GYRO_NORMAL (0<<7) | (0<<5)
#define BMI055_GYRO_DEEP_SUSPEND (0<<7) | (1<<5)
#define BMI055_GYRO_SUSPEND (1<<7) | (0<<5)
//BMI055_GYR_RANGE 0x0F
#define BMI055_GYRO_RANGE_2000_DPS (0<<2) | (0<<1) | (0<<0)
#define BMI055_GYRO_RANGE_1000_DPS (0<<2) | (0<<1) | (1<<0)
#define BMI055_GYRO_RANGE_500_DPS (0<<2) | (1<<1) | (0<<0)
#define BMI055_GYRO_RANGE_250_DPS (0<<2) | (1<<1) | (1<<0)
#define BMI055_GYRO_RANGE_125_DPS (1<<2) | (0<<1) | (0<<0)
//BMI055_ACC_INT_EN_1 0x17
#define BMI055_ACC_DRDY_INT_EN (1<<4)
//BMI055_GYR_INT_EN_0 0x15
#define BMI055_GYR_DRDY_INT_EN (1<<7)
//BMI055_ACC_INT_MAP_1 0x1A
#define BMI055_ACC_DRDY_INT1 (1<<0)
//BMI055_GYR_INT_MAP_1 0x18
#define BMI055_GYR_DRDY_INT1 (1<<0)
//Soft-reset command Value
#define BMI055_SOFT_RESET 0xB6
// Default and Max values
#define BMI055_ACCEL_DEFAULT_RANGE_G 16
#define BMI055_GYRO_DEFAULT_RANGE_DPS 2000
#define BMI055_ACCEL_DEFAULT_RATE 1000
#define BMI055_ACCEL_MAX_RATE 1000
#define BMI055_ACCEL_MAX_PUBLISH_RATE 280
#define BMI055_GYRO_DEFAULT_RATE 1000
#define BMI055_GYRO_MAX_RATE 1000
#define BMI055_GYRO_MAX_PUBLISH_RATE BMI055_ACCEL_MAX_PUBLISH_RATE
#define BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 50
#define BMI055_GYRO_DEFAULT_DRIVER_FILTER_FREQ 50
#define BMI055_ONE_G 9.80665f
#define BMI055_BUS_SPEED 10*1000*1000
#define BMI055_TIMER_REDUCTION 200
/* Mask definitions for ACCD_X_LSB, ACCD_Y_LSB and ACCD_Z_LSB Register */
#define BMI055_NEW_DATA_MASK 0x01
/* Mask definitions for Gyro bandwidth */
#define BMI055_GYRO_BW_MASK 0x0F
class BMI055 : public device::SPI
{
protected:
uint8_t _whoami; /** whoami result */
struct hrt_call _call;
unsigned _call_interval;
unsigned _dlpf_freq;
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
perf_counter_t _bad_registers;
perf_counter_t _good_transfers;
perf_counter_t _reset_retries;
perf_counter_t _duplicates;
perf_counter_t _controller_latency_perf;
uint8_t _register_wait;
uint64_t _reset_wait;
enum Rotation _rotation;
uint8_t _checked_next;
/**
* Read a register from the BMI055
*
* @param The register to read.
* @return The value that was read.
*/
uint8_t read_reg(unsigned reg);
uint16_t read_reg16(unsigned reg);
/**
* Write a register in the BMI055
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_reg(unsigned reg, uint8_t value);
/* do not allow to copy this class due to pointer data members */
BMI055(const BMI055 &);
BMI055 operator=(const BMI055 &);
public:
BMI055(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency,
enum Rotation rotation);
virtual ~BMI055();
};
class BMI055_accel : public BMI055
{
public:
BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation);
virtual ~BMI055_accel();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
void print_registers();
// deliberately cause a sensor error
void test_error();
protected:
virtual int probe();
private:
ringbuffer::RingBuffer *_accel_reports;
struct accel_calibration_s _accel_scale;
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
int _accel_orb_class_instance;
int _accel_class_instance;
float _accel_sample_rate;
perf_counter_t _accel_reads;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
math::LowPassFilter2p _accel_filter_z;
Integrator _accel_int;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define BMI055_ACCEL_NUM_CHECKED_REGISTERS 5
static const uint8_t _checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[BMI055_ACCEL_NUM_CHECKED_REGISTERS];
uint8_t _checked_bad[BMI055_ACCEL_NUM_CHECKED_REGISTERS];
// last temperature reading for print_info()
float _last_temperature;
bool _got_duplicate;
/**
* Start automatic measurement.
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Reset chip.
*
* Resets the chip and measurements ranges, but not scale and offset.
*/
int reset();
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
*
* Called by the HRT in interrupt context at the specified rate if
* automatic polling is enabled.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void measure_trampoline(void *arg);
/**
* Fetch measurements from the sensor and update the report buffers.
*/
void measure();
/**
* Modify a register in the BMI055_accel
*
* Bits are cleared before bits are set.
*
* @param reg The register to modify.
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Write a register in the BMI055_accel, updating _checked_values
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_checked_reg(unsigned reg, uint8_t value);
/**
* Set the BMI055_accel measurement range.
*
* @param max_g The maximum G value the range must support.
* @return OK if the value can be supported, -EINVAL otherwise.
*/
int set_accel_range(unsigned max_g);
/**
* Measurement self test
*
* @return 0 on success, 1 on failure
*/
int self_test();
/**
* Accel self test
*
* @return 0 on success, 1 on failure
*/
int accel_self_test();
/*
set accel sample rate
*/
int accel_set_sample_rate(float desired_sample_rate_hz);
/*
check that key registers still have the right value
*/
void check_registers(void);
/* do not allow to copy this class due to pointer data members */
BMI055_accel(const BMI055_accel &);
BMI055_accel operator=(const BMI055_accel &);
};
class BMI055_gyro : public BMI055
{
public:
BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation);
virtual ~BMI055_gyro();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
void print_registers();
// deliberately cause a sensor error
void test_error();
protected:
virtual int probe();
private:
ringbuffer::RingBuffer *_gyro_reports;
struct gyro_calibration_s _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
int _gyro_orb_class_instance;
int _gyro_class_instance;
float _gyro_sample_rate;
perf_counter_t _gyro_reads;
math::LowPassFilter2p _gyro_filter_x;
math::LowPassFilter2p _gyro_filter_y;
math::LowPassFilter2p _gyro_filter_z;
Integrator _gyro_int;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define BMI055_GYRO_NUM_CHECKED_REGISTERS 7
static const uint8_t _checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[BMI055_GYRO_NUM_CHECKED_REGISTERS];
uint8_t _checked_bad[BMI055_GYRO_NUM_CHECKED_REGISTERS];
// last temperature reading for print_info()
float _last_temperature;
/**
* Start automatic measurement.
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Reset chip.
*
* Resets the chip and measurements ranges, but not scale and offset.
*/
int reset();
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
*
* Called by the HRT in interrupt context at the specified rate if
* automatic polling is enabled.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void measure_trampoline(void *arg);
/**
* Fetch measurements from the sensor and update the report buffers.
*/
void measure();
/**
* Modify a register in the BMI055_gyro
*
* Bits are cleared before bits are set.
*
* @param reg The register to modify.
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Write a register in the BMI055_gyro, updating _checked_values
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_checked_reg(unsigned reg, uint8_t value);
/**
* Set the BMI055_gyro measurement range.
*
* @param max_dps The maximum DPS value the range must support.
* @return OK if the value can be supported, -EINVAL otherwise.
*/
int set_gyro_range(unsigned max_dps);
/**
* Measurement self test
*
* @return 0 on success, 1 on failure
*/
int self_test();
/**
* Gyro self test
*
* @return 0 on success, 1 on failure
*/
int gyro_self_test();
/*
* set gyro sample rate
*/
int gyro_set_sample_rate(float desired_sample_rate_hz);
/*
* check that key registers still have the right value
*/
void check_registers(void);
/* do not allow to copy this class due to pointer data members */
BMI055_gyro(const BMI055_gyro &);
BMI055_gyro operator=(const BMI055_gyro &);
#pragma pack(push, 1)
/**
* Report conversation within the BMI055_gyro, including command byte and
* interrupt status.
*/
struct BMI_GyroReport {
uint8_t cmd;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
};
#pragma pack(pop)
};
#endif /* BMI055_HPP_ */
src/drivers/imu/bmi055/bmi055_accel.cpp
+867
View File
@@ -0,0 +1,867 @@
#include "bmi055.hpp"
/*
list of registers that will be checked in check_registers(). Note
that ADDR_WHO_AM_I must be first in the list.
*/
const uint8_t BMI055_accel::_checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTERS] = { BMI055_ACC_CHIP_ID,
BMI055_ACC_BW,
BMI055_ACC_RANGE,
BMI055_ACC_INT_EN_1,
BMI055_ACC_INT_MAP_1,
};
BMI055_accel::BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation) :
BMI055("BMI055_ACCEL", path_accel, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation),
_accel_reports(nullptr),
_accel_scale{},
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(nullptr),
_accel_orb_class_instance(-1),
_accel_class_instance(-1),
_accel_sample_rate(BMI055_ACCEL_DEFAULT_RATE),
_accel_reads(perf_alloc(PC_COUNT, "bmi055_accel_read")),
_accel_filter_x(BMI055_ACCEL_DEFAULT_RATE, BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(BMI055_ACCEL_DEFAULT_RATE, BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(BMI055_ACCEL_DEFAULT_RATE, BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_int(1000000 / BMI055_ACCEL_MAX_PUBLISH_RATE),
_last_temperature(0),
_got_duplicate(false)
{
// disable debug() calls
_debug_enabled = false;
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_BMI055;
// 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;
memset(&_call, 0, sizeof(_call));
}
BMI055_accel::~BMI055_accel()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_accel_reports != nullptr) {
delete _accel_reports;
}
if (_accel_class_instance != -1) {
unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance);
}
/* delete the perf counter */
perf_free(_accel_reads);
}
int
BMI055_accel::init()
{
int ret;
/* do SPI init (and probe) first */
ret = SPI::init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("SPI setup failed");
return ret;
}
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr) {
goto out;
}
if (reset() != OK) {
goto out;
}
/* 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;
_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
measure();
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel_orb_class_instance, (external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_accel_topic == nullptr) {
warnx("ADVERT FAIL");
}
out:
return ret;
}
int BMI055_accel::reset()
{
write_reg(BMI055_ACC_SOFTRESET, BMI055_SOFT_RESET);//Soft-reset
up_udelay(5000);
write_checked_reg(BMI055_ACC_BW, BMI055_ACCEL_BW_1000); //Write accel bandwidth
write_checked_reg(BMI055_ACC_RANGE, BMI055_ACCEL_RANGE_2_G);//Write range
write_checked_reg(BMI055_ACC_INT_EN_1, BMI055_ACC_DRDY_INT_EN); //Enable DRDY interrupt
write_checked_reg(BMI055_ACC_INT_MAP_1, BMI055_ACC_DRDY_INT1); //Map DRDY interrupt on pin INT1
set_accel_range(BMI055_ACCEL_DEFAULT_RANGE_G);//set accel range
accel_set_sample_rate(BMI055_ACCEL_DEFAULT_RATE);//set accel ODR
//Enable Accelerometer in normal mode
write_reg(BMI055_ACC_PMU_LPW, BMI055_ACCEL_NORMAL);
up_udelay(1000);
uint8_t retries = 10;
while (retries--) {
bool all_ok = true;
for (uint8_t i = 0; i < BMI055_ACCEL_NUM_CHECKED_REGISTERS; i++) {
if (read_reg(_checked_registers[i]) != _checked_values[i]) {
write_reg(_checked_registers[i], _checked_values[i]);
all_ok = false;
}
}
if (all_ok) {
break;
}
}
_accel_reads = 0;
return OK;
}
int
BMI055_accel::probe()
{
/* look for device ID */
_whoami = read_reg(BMI055_ACC_CHIP_ID);
// verify product revision
switch (_whoami) {
case BMI055_ACC_WHO_AM_I:
memset(_checked_values, 0, sizeof(_checked_values));
memset(_checked_bad, 0, sizeof(_checked_bad));
_checked_values[0] = _whoami;
_checked_bad[0] = _whoami;
return OK;
}
DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami);
return -EIO;
}
int
BMI055_accel::accel_set_sample_rate(float frequency)
{
uint8_t setbits = 0;
uint8_t clearbits = BMI055_ACCEL_BW_1000;
if (frequency < (3125 / 100)) {
setbits |= BMI055_ACCEL_BW_7_81;
_accel_sample_rate = 1563 / 100;
} else if (frequency < (625 / 10)) {
setbits |= BMI055_ACCEL_BW_15_63;
_accel_sample_rate = 625 / 10;
} else if (frequency < (125)) {
setbits |= BMI055_ACCEL_BW_31_25;
_accel_sample_rate = 625 / 10;
} else if (frequency < 250) {
setbits |= BMI055_ACCEL_BW_62_5;
_accel_sample_rate = 125;
} else if (frequency < 500) {
setbits |= BMI055_ACCEL_BW_125;
_accel_sample_rate = 250;
} else if (frequency < 1000) {
setbits |= BMI055_ACCEL_BW_250;
_accel_sample_rate = 500;
} else if (frequency < 2000) {
setbits |= BMI055_ACCEL_BW_500;
_accel_sample_rate = 1000;
} else if (frequency >= 2000) {
setbits |= BMI055_ACCEL_BW_1000;
_accel_sample_rate = 2000;
} else {
return -EINVAL;
}
/* Write accel ODR */
modify_reg(BMI055_ACC_BW, clearbits, setbits);
return OK;
}
ssize_t
BMI055_accel::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(accel_report);
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is not enabled, get a fresh measurement into the buffer */
if (_call_interval == 0) {
_accel_reports->flush();
measure();
}
/* if no data, error (we could block here) */
if (_accel_reports->empty()) {
return -EAGAIN;
}
perf_count(_accel_reads);
/* copy reports out of our buffer to the caller */
accel_report *arp = reinterpret_cast<accel_report *>(buffer);
int transferred = 0;
while (count--) {
if (!_accel_reports->get(arp)) {
break;
}
transferred++;
arp++;
}
/* return the number of bytes transferred */
return (transferred * sizeof(accel_report));
}
int
BMI055_accel::self_test()
{
if (perf_event_count(_sample_perf) == 0) {
measure();
}
/* return 0 on success, 1 else */
return (perf_event_count(_sample_perf) > 0) ? 0 : 1;
}
int
BMI055_accel::accel_self_test()
{
if (self_test()) {
return 1;
}
/* inspect accel offsets */
if (fabsf(_accel_scale.x_offset) < 0.000001f) {
return 1;
}
if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) {
return 1;
}
if (fabsf(_accel_scale.y_offset) < 0.000001f) {
return 1;
}
if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) {
return 1;
}
if (fabsf(_accel_scale.z_offset) < 0.000001f) {
return 1;
}
if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) {
return 1;
}
return 0;
}
/*
deliberately trigger an error in the sensor to trigger recovery
*/
void
BMI055_accel::test_error()
{
write_reg(BMI055_ACC_SOFTRESET, BMI055_SOFT_RESET);
::printf("error triggered\n");
print_registers();
}
int
BMI055_accel::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCRESET:
return reset();
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_call_interval = 0;
return OK;
/* external signalling not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
return ioctl(filp, SENSORIOCSPOLLRATE, BMI055_ACCEL_MAX_RATE);
case SENSOR_POLLRATE_DEFAULT:
return ioctl(filp, SENSORIOCSPOLLRATE,
BMI055_ACCEL_DEFAULT_RATE); //Polling at the highest frequency. We may get duplicate values on the sensors
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_call_interval == 0);
/* convert hz to hrt interval via microseconds */
unsigned ticks = 1000000 / arg;
/* check against maximum rate */
if (ticks < 1000) {
return -EINVAL;
}
// adjust filters
float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
float sample_rate = 1.0e6f / ticks;
_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);
/* update interval for next measurement */
_call_interval = ticks;
/*
set call interval faster than the sample time. We
then detect when we have duplicate samples and reject
them. This prevents aliasing due to a beat between the
stm32 clock and the bmi055 clock
*/
_call.period = _call_interval - BMI055_TIMER_REDUCTION;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_call_interval == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return 1000000 / _call_interval;
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = px4_enter_critical_section();
if (!_accel_reports->resize(arg)) {
px4_leave_critical_section(flags);
return -ENOMEM;
}
px4_leave_critical_section(flags);
return OK;
}
case ACCELIOCGSAMPLERATE:
return _accel_sample_rate;
case ACCELIOCSSAMPLERATE:
return accel_set_sample_rate(arg);
case ACCELIOCSSCALE: {
/* copy scale, but only if off by a few percent */
struct accel_calibration_s *s = (struct accel_calibration_s *) arg;
float sum = s->x_scale + s->y_scale + s->z_scale;
if (sum > 2.0f && sum < 4.0f) {
memcpy(&_accel_scale, s, sizeof(_accel_scale));
return OK;
} else {
return -EINVAL;
}
}
case ACCELIOCGSCALE:
/* copy scale out */
memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale));
return OK;
case ACCELIOCSRANGE:
return set_accel_range(arg);
case ACCELIOCGRANGE:
return (unsigned long)((_accel_range_m_s2) / BMI055_ONE_G + 0.5f);
case ACCELIOCSELFTEST:
return accel_self_test();
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
}
}
void
BMI055_accel::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
{
uint8_t val;
val = read_reg(reg);
val &= ~clearbits;
val |= setbits;
write_checked_reg(reg, val);
}
void
BMI055_accel::write_checked_reg(unsigned reg, uint8_t value)
{
write_reg(reg, value);
for (uint8_t i = 0; i < BMI055_ACCEL_NUM_CHECKED_REGISTERS; i++) {
if (reg == _checked_registers[i]) {
_checked_values[i] = value;
_checked_bad[i] = value;
}
}
}
int
BMI055_accel::set_accel_range(unsigned max_g)
{
uint8_t setbits = 0;
uint8_t clearbits = BMI055_ACCEL_RANGE_2_G | BMI055_ACCEL_RANGE_16_G;
float lsb_per_g;
float max_accel_g;
if (max_g == 0) {
max_g = 16;
}
if (max_g <= 2) {
max_accel_g = 2;
setbits |= BMI055_ACCEL_RANGE_2_G;
lsb_per_g = 1024;
} else if (max_g <= 4) {
max_accel_g = 4;
setbits |= BMI055_ACCEL_RANGE_4_G;
lsb_per_g = 512;
} else if (max_g <= 8) {
max_accel_g = 8;
setbits |= BMI055_ACCEL_RANGE_8_G;
lsb_per_g = 256;
} else if (max_g <= 16) {
max_accel_g = 16;
setbits |= BMI055_ACCEL_RANGE_16_G;
lsb_per_g = 128;
} else {
return -EINVAL;
}
_accel_range_scale = (BMI055_ONE_G / lsb_per_g);
_accel_range_m_s2 = max_accel_g * BMI055_ONE_G;
modify_reg(BMI055_ACC_RANGE, clearbits, setbits);
return OK;
}
void
BMI055_accel::start()
{
/* make sure we are stopped first */
stop();
/* discard any stale data in the buffers */
_accel_reports->flush();
/* start polling at the specified rate */
hrt_call_every(&_call,
1000,
_call_interval - BMI055_TIMER_REDUCTION,
(hrt_callout)&BMI055_accel::measure_trampoline, this);
reset();
}
void
BMI055_accel::stop()
{
hrt_cancel(&_call);
}
void
BMI055_accel::measure_trampoline(void *arg)
{
BMI055_accel *dev = reinterpret_cast<BMI055_accel *>(arg);
/* make another measurement */
dev->measure();
}
void
BMI055_accel::check_registers(void)
{
uint8_t v;
if ((v = read_reg(_checked_registers[_checked_next])) !=
_checked_values[_checked_next]) {
_checked_bad[_checked_next] = v;
/*
if we get the wrong value then we know the SPI bus
or sensor is very sick. We set _register_wait to 20
and wait until we have seen 20 good values in a row
before we consider the sensor to be OK again.
*/
perf_count(_bad_registers);
/*
try to fix the bad register value. We only try to
fix one per loop to prevent a bad sensor hogging the
bus.
*/
if (_register_wait == 0 || _checked_next == 0) {
// if the product_id is wrong then reset the
// sensor completely
write_reg(BMI055_ACC_SOFTRESET, BMI055_SOFT_RESET);
_reset_wait = hrt_absolute_time() + 10000;
_checked_next = 0;
} else {
write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
// waiting 3ms between register writes seems
// to raise the chance of the sensor
// recovering considerably
_reset_wait = hrt_absolute_time() + 3000;
}
_register_wait = 20;
}
_checked_next = (_checked_next + 1) % BMI055_ACCEL_NUM_CHECKED_REGISTERS;
}
void
BMI055_accel::measure()
{
uint8_t index = 0, accel_data[7];
uint16_t lsb, msb, msblsb;
uint8_t status_x, status_y, status_z;
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
return;
}
struct Report {
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t temp;
} report;
/* start measuring */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the BMI055 in one pass.
*/
accel_data[index] = BMI055_ACC_X_L | DIR_READ;
if (OK != transfer(accel_data, accel_data, sizeof(accel_data))) {
return;
}
check_registers();
/* Extracting accel data from the read data */
index = 1;
lsb = (uint16_t)accel_data[index++];
status_x = (lsb & BMI055_NEW_DATA_MASK);
msb = (uint16_t)accel_data[index++];
msblsb = (msb << 8) | lsb;
report.accel_x = ((int16_t)msblsb >> 4); /* Data in X axis */
lsb = (uint16_t)accel_data[index++];
status_y = (lsb & BMI055_NEW_DATA_MASK);
msb = (uint16_t)accel_data[index++];
msblsb = (msb << 8) | lsb;
report.accel_y = ((int16_t)msblsb >> 4); /* Data in Y axis */
lsb = (uint16_t)accel_data[index++];
status_z = (lsb & BMI055_NEW_DATA_MASK);
msb = (uint16_t)accel_data[index++];
msblsb = (msb << 8) | lsb;
report.accel_z = ((int16_t)msblsb >> 4); /* Data in Z axis */
// Checking the status of new data
if ((!status_x) || (!status_y) || (!status_z)) {
perf_end(_sample_perf);
perf_count(_duplicates);
_got_duplicate = true;
return;
}
_got_duplicate = false;
uint8_t temp = read_reg(BMI055_ACC_TEMP);
report.temp = temp;
if (report.accel_x == 0 &&
report.accel_y == 0 &&
report.accel_z == 0 &&
report.temp == 0) {
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
// note that we don't call reset() here as a reset()
// costs 20ms with interrupts disabled. That means if
// the bmi055 accel does go bad it would cause a FMU failure,
// regardless of whether another sensor is available,
return;
}
perf_count(_good_transfers);
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again. We still increment
// _good_transfers, but don't return any data yet
_register_wait--;
return;
}
/*
* Report buffers.
*/
accel_report arb;
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
arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
*/
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);
math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
math::Vector<3> 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.range_m_s2 = _accel_range_m_s2;
_last_temperature = 23 + report.temp * 1.0f / 512.0f;
arb.temperature_raw = report.temp;
arb.temperature = _last_temperature;
arb.device_id = _device_id.devid;
_accel_reports->force(&arb);
/* notify anyone waiting for data */
if (accel_notify) {
poll_notify(POLLIN);
}
if (accel_notify && !(_pub_blocked)) {
/* log the time of this report */
perf_begin(_controller_latency_perf);
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
/* stop measuring */
perf_end(_sample_perf);
}
void
BMI055_accel::print_info()
{
warnx("BMI055 Accel");
perf_print_counter(_sample_perf);
perf_print_counter(_accel_reads);
perf_print_counter(_bad_transfers);
perf_print_counter(_bad_registers);
perf_print_counter(_good_transfers);
perf_print_counter(_reset_retries);
perf_print_counter(_duplicates);
_accel_reports->print_info("accel queue");
::printf("checked_next: %u\n", _checked_next);
for (uint8_t i = 0; i < BMI055_ACCEL_NUM_CHECKED_REGISTERS; i++) {
uint8_t v = read_reg(_checked_registers[i]);
if (v != _checked_values[i]) {
::printf("reg %02x:%02x should be %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_values[i]);
}
if (v != _checked_bad[i]) {
::printf("reg %02x:%02x was bad %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_bad[i]);
}
}
::printf("temperature: %.1f\n", (double)_last_temperature);
printf("\n");
}
void
BMI055_accel::print_registers()
{
uint8_t index = 0;
printf("BMI055 accel registers\n");
uint8_t reg = _checked_registers[index++];
uint8_t v = read_reg(reg);
printf("Accel Chip Id: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Accel Bw: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Accel Range: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Accel Int-en-1: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Accel Int-Map-1: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
}
src/drivers/imu/bmi055/bmi055_gyro.cpp
+848
View File
@@ -0,0 +1,848 @@
#include "bmi055.hpp"
/*
list of registers that will be checked in check_registers(). Note
that ADDR_WHO_AM_I must be first in the list.
*/
const uint8_t BMI055_gyro::_checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS] = { BMI055_GYR_CHIP_ID,
BMI055_GYR_LPM1,
BMI055_GYR_BW,
BMI055_GYR_RANGE,
BMI055_GYR_INT_EN_0,
BMI055_GYR_INT_EN_1,
BMI055_GYR_INT_MAP_1
};
BMI055_gyro::BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation) :
BMI055("BMI055_GYRO", path_gyro, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation),
_gyro_reports(nullptr),
_gyro_scale{},
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_gyro_topic(nullptr),
_gyro_orb_class_instance(-1),
_gyro_class_instance(-1),
_gyro_sample_rate(BMI055_GYRO_DEFAULT_RATE),
_gyro_reads(perf_alloc(PC_COUNT, "bmi055_gyro_read")),
_gyro_filter_x(BMI055_GYRO_DEFAULT_RATE, BMI055_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_y(BMI055_GYRO_DEFAULT_RATE, BMI055_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_z(BMI055_GYRO_DEFAULT_RATE, BMI055_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_int(1000000 / BMI055_GYRO_MAX_PUBLISH_RATE, true),
_last_temperature(0)
{
// disable debug() calls
_debug_enabled = false;
_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_BMI055;
// 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;
memset(&_call, 0, sizeof(_call));
}
BMI055_gyro::~BMI055_gyro()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_gyro_reports != nullptr) {
delete _gyro_reports;
}
if (_gyro_class_instance != -1) {
unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance);
}
/* delete the perf counter */
perf_free(_gyro_reads);
}
int
BMI055_gyro::init()
{
int ret;
/* do SPI init (and probe) first */
ret = SPI::init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("SPI setup failed");
return ret;
}
/* allocate basic report buffers */
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_gyro_reports == nullptr) {
goto out;
}
if (reset() != OK) {
goto out;
}
/* Initialize offsets and scales */
_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;
/* 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);
measure();
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_gyro_reports->get(&grp);
_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_gyro_orb_class_instance, (external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_gyro_topic == nullptr) {
warnx("ADVERT FAIL");
}
out:
return ret;
}
int BMI055_gyro::reset()
{
write_reg(BMI055_GYR_SOFTRESET, BMI055_SOFT_RESET);//Soft-reset
usleep(5000);
write_checked_reg(BMI055_GYR_BW, 0); // Write Gyro Bandwidth
write_checked_reg(BMI055_GYR_RANGE, 0);// Write Gyro range
write_checked_reg(BMI055_GYR_INT_EN_0, BMI055_GYR_DRDY_INT_EN); //Enable DRDY interrupt
write_checked_reg(BMI055_GYR_INT_MAP_1, BMI055_GYR_DRDY_INT1); //Map DRDY interrupt on pin INT1
set_gyro_range(BMI055_GYRO_DEFAULT_RANGE_DPS);// set Gyro range
gyro_set_sample_rate(BMI055_GYRO_DEFAULT_RATE);// set Gyro ODR
//Enable Gyroscope in normal mode
write_reg(BMI055_GYR_LPM1, BMI055_GYRO_NORMAL);
up_udelay(1000);
uint8_t retries = 10;
while (retries--) {
bool all_ok = true;
for (uint8_t i = 0; i < BMI055_GYRO_NUM_CHECKED_REGISTERS; i++) {
if (read_reg(_checked_registers[i]) != _checked_values[i]) {
write_reg(_checked_registers[i], _checked_values[i]);
all_ok = false;
}
}
if (all_ok) {
break;
}
}
_gyro_reads = 0;
return OK;
}
int
BMI055_gyro::probe()
{
/* look for device ID */
_whoami = read_reg(BMI055_GYR_CHIP_ID);
// verify product revision
switch (_whoami) {
case BMI055_GYR_WHO_AM_I:
memset(_checked_values, 0, sizeof(_checked_values));
memset(_checked_bad, 0, sizeof(_checked_bad));
_checked_values[0] = _whoami;
_checked_bad[0] = _whoami;
return OK;
}
DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami);
return -EIO;
}
int
BMI055_gyro::gyro_set_sample_rate(float frequency)
{
uint8_t setbits = 0;
uint8_t clearbits = BMI055_GYRO_BW_MASK;
if (frequency <= 100) {
setbits |= BMI055_GYRO_RATE_100;
_gyro_sample_rate = 100;
} else if (frequency <= 250) {
setbits |= BMI055_GYRO_RATE_400;
_gyro_sample_rate = 400;
} else if (frequency <= 1000) {
setbits |= BMI055_GYRO_RATE_1000;
_gyro_sample_rate = 1000;
} else if (frequency > 1000) {
setbits |= BMI055_GYRO_RATE_2000;
_gyro_sample_rate = 2000;
} else {
return -EINVAL;
}
modify_reg(BMI055_GYR_BW, clearbits, setbits);
return OK;
}
int
BMI055_gyro::self_test()
{
if (perf_event_count(_sample_perf) == 0) {
measure();
}
/* return 0 on success, 1 else */
return (perf_event_count(_sample_perf) > 0) ? 0 : 1;
}
int
BMI055_gyro::gyro_self_test()
{
if (self_test()) {
return 1;
}
/*
* Maximum deviation of 10 degrees
*/
const float max_offset = (float)(10 * M_PI_F / 180.0f);
/* 30% scale error is chosen to catch completely faulty units but
* to let some slight scale error pass. Requires a rate table or correlation
* with mag rotations + data fit to
* calibrate properly and is not done by default.
*/
const float max_scale = 0.3f;
/* evaluate gyro offsets, complain if offset -> zero or larger than 30 dps. */
if (fabsf(_gyro_scale.x_offset) > max_offset) {
return 1;
}
/* evaluate gyro scale, complain if off by more than 30% */
if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) {
return 1;
}
if (fabsf(_gyro_scale.y_offset) > max_offset) {
return 1;
}
if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) {
return 1;
}
if (fabsf(_gyro_scale.z_offset) > max_offset) {
return 1;
}
if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) {
return 1;
}
/* check if all scales are zero */
if ((fabsf(_gyro_scale.x_offset) < 0.000001f) &&
(fabsf(_gyro_scale.y_offset) < 0.000001f) &&
(fabsf(_gyro_scale.z_offset) < 0.000001f)) {
/* if all are zero, this device is not calibrated */
return 1;
}
return 0;
}
/*
deliberately trigger an error in the sensor to trigger recovery
*/
void
BMI055_gyro::test_error()
{
write_reg(BMI055_GYR_SOFTRESET, BMI055_SOFT_RESET);
::printf("error triggered\n");
print_registers();
}
ssize_t
BMI055_gyro::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(gyro_report);
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is not enabled, get a fresh measurement into the buffer */
if (_call_interval == 0) {
_gyro_reports->flush();
measure();
}
/* if no data, error (we could block here) */
if (_gyro_reports->empty()) {
return -EAGAIN;
}
perf_count(_gyro_reads);
/* copy reports out of our buffer to the caller */
gyro_report *grp = reinterpret_cast<gyro_report *>(buffer);
int transferred = 0;
while (count--) {
if (!_gyro_reports->get(grp)) {
break;
}
transferred++;
grp++;
}
/* return the number of bytes transferred */
return (transferred * sizeof(gyro_report));
}
int
BMI055_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_call_interval = 0;
return OK;
/* external signalling not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
return ioctl(filp, SENSORIOCSPOLLRATE, BMI055_GYRO_MAX_RATE);
case SENSOR_POLLRATE_DEFAULT:
return ioctl(filp, SENSORIOCSPOLLRATE, BMI055_GYRO_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_call_interval == 0);
/* convert hz to hrt interval via microseconds */
unsigned ticks = 1000000 / arg;
/* check against maximum rate */
if (ticks < 1000) {
return -EINVAL;
}
float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq();
float sample_rate = 1.0e6f / ticks;
_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 */
_call_interval = ticks;
/*
set call interval faster than the sample time. We
then detect when we have duplicate samples and reject
them. This prevents aliasing due to a beat between the
stm32 clock and the bmi055 clock
*/
_call.period = _call_interval - BMI055_TIMER_REDUCTION;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_call_interval == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return 1000000 / _call_interval;
case SENSORIOCRESET:
return reset();
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = px4_enter_critical_section();
if (!_gyro_reports->resize(arg)) {
px4_leave_critical_section(flags);
return -ENOMEM;
}
px4_leave_critical_section(flags);
return OK;
}
case GYROIOCGSAMPLERATE:
return _gyro_sample_rate;
case GYROIOCSSAMPLERATE:
return gyro_set_sample_rate(arg);
case GYROIOCSSCALE:
/* copy scale in */
memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale));
return OK;
case GYROIOCGSCALE:
/* copy scale out */
memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale));
return OK;
case GYROIOCSRANGE:
return set_gyro_range(arg);
case GYROIOCGRANGE:
return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f);
case GYROIOCSELFTEST:
return gyro_self_test();
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
}
}
void
BMI055_gyro::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
{
uint8_t val;
val = read_reg(reg);
val &= ~clearbits;
val |= setbits;
write_checked_reg(reg, val);
}
void
BMI055_gyro::write_checked_reg(unsigned reg, uint8_t value)
{
write_reg(reg, value);
for (uint8_t i = 0; i < BMI055_GYRO_NUM_CHECKED_REGISTERS; i++) {
if (reg == _checked_registers[i]) {
_checked_values[i] = value;
_checked_bad[i] = value;
}
}
}
int
BMI055_gyro::set_gyro_range(unsigned max_dps)
{
uint8_t setbits = 0;
uint8_t clearbits = BMI055_GYRO_RANGE_125_DPS | BMI055_GYRO_RANGE_250_DPS;
float lsb_per_dps;
float max_gyro_dps;
if (max_dps == 0) {
max_dps = 2000;
}
if (max_dps <= 125) {
max_gyro_dps = 125;
lsb_per_dps = 262.4;
setbits |= BMI055_GYRO_RANGE_125_DPS;
} else if (max_dps <= 250) {
max_gyro_dps = 250;
lsb_per_dps = 131.2;
setbits |= BMI055_GYRO_RANGE_250_DPS;
} else if (max_dps <= 500) {
max_gyro_dps = 500;
lsb_per_dps = 65.6;
setbits |= BMI055_GYRO_RANGE_500_DPS;
} else if (max_dps <= 1000) {
max_gyro_dps = 1000;
lsb_per_dps = 32.8;
setbits |= BMI055_GYRO_RANGE_1000_DPS;
} else if (max_dps <= 2000) {
max_gyro_dps = 2000;
lsb_per_dps = 16.4;
setbits |= BMI055_GYRO_RANGE_2000_DPS;
} else {
return -EINVAL;
}
_gyro_range_rad_s = (max_gyro_dps / 180.0f * M_PI_F);
_gyro_range_scale = (M_PI_F / (180.0f * lsb_per_dps));
modify_reg(BMI055_GYR_RANGE, clearbits, setbits);
return OK;
}
void
BMI055_gyro::start()
{
/* make sure we are stopped first */
stop();
/* discard any stale data in the buffers */
_gyro_reports->flush();
/* start polling at the specified rate */
hrt_call_every(&_call,
1000,
_call_interval - BMI055_TIMER_REDUCTION,
(hrt_callout)&BMI055_gyro::measure_trampoline, this);
reset();
}
void
BMI055_gyro::stop()
{
hrt_cancel(&_call);
}
void
BMI055_gyro::measure_trampoline(void *arg)
{
BMI055_gyro *dev = reinterpret_cast<BMI055_gyro *>(arg);
/* make another measurement */
dev->measure();
}
void
BMI055_gyro::check_registers(void)
{
uint8_t v;
if ((v = read_reg(_checked_registers[_checked_next])) !=
_checked_values[_checked_next]) {
_checked_bad[_checked_next] = v;
/*
if we get the wrong value then we know the SPI bus
or sensor is very sick. We set _register_wait to 20
and wait until we have seen 20 good values in a row
before we consider the sensor to be OK again.
*/
perf_count(_bad_registers);
/*
try to fix the bad register value. We only try to
fix one per loop to prevent a bad sensor hogging the
bus.
*/
if (_register_wait == 0 || _checked_next == 0) {
// if the product_id is wrong then reset the
// sensor completely
write_reg(BMI055_GYR_SOFTRESET, BMI055_SOFT_RESET);
_reset_wait = hrt_absolute_time() + 10000;
_checked_next = 0;
} else {
write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
// waiting 3ms between register writes seems
// to raise the chance of the sensor
// recovering considerably
_reset_wait = hrt_absolute_time() + 3000;
}
_register_wait = 20;
}
_checked_next = (_checked_next + 1) % BMI055_GYRO_NUM_CHECKED_REGISTERS;
}
void
BMI055_gyro::measure()
{
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
return;
}
struct BMI_GyroReport bmi_gyroreport;
struct Report {
int16_t temp;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
} report;
/* start measuring */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the BMI055 gyro in one pass.
*/
bmi_gyroreport.cmd = BMI055_GYR_X_L | DIR_READ;
if (OK != transfer((uint8_t *)&bmi_gyroreport, ((uint8_t *)&bmi_gyroreport), sizeof(bmi_gyroreport))) {
return;
}
check_registers();
uint8_t temp = read_reg(BMI055_ACC_TEMP);
report.temp = temp;
report.gyro_x = bmi_gyroreport.gyro_x;
report.gyro_y = bmi_gyroreport.gyro_y;
report.gyro_z = bmi_gyroreport.gyro_z;
if (report.temp == 0 &&
report.gyro_x == 0 &&
report.gyro_y == 0 &&
report.gyro_z == 0) {
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
// note that we don't call reset() here as a reset()
// costs 20ms with interrupts disabled. That means if
// the bmi055 does go bad it would cause a FMU failure,
// regardless of whether another sensor is available,
return;
}
perf_count(_good_transfers);
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again. We still increment
// _good_transfers, but don't return any data yet
_register_wait--;
return;
}
/*
* Report buffers.
*/
gyro_report grb;
grb.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 = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
grb.x_raw = report.gyro_x;
grb.y_raw = report.gyro_y;
grb.z_raw = report.gyro_z;
float xraw_f = report.gyro_x;
float yraw_f = report.gyro_y;
float 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);
math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
math::Vector<3> gval_integrated;
bool gyro_notify = _gyro_int.put(grb.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.range_rad_s = _gyro_range_rad_s;
grb.temperature_raw = report.temp;
grb.temperature = _last_temperature;
grb.device_id = _device_id.devid;
_gyro_reports->force(&grb);
/* notify anyone waiting for data */
if (gyro_notify) {
poll_notify(POLLIN);
}
if (gyro_notify && !(_pub_blocked)) {
/* log the time of this report */
perf_begin(_controller_latency_perf);
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb);
}
/* stop measuring */
perf_end(_sample_perf);
}
void
BMI055_gyro::print_info()
{
warnx("BMI055 Gyro");
perf_print_counter(_sample_perf);
perf_print_counter(_gyro_reads);
perf_print_counter(_bad_transfers);
perf_print_counter(_bad_registers);
perf_print_counter(_good_transfers);
perf_print_counter(_reset_retries);
perf_print_counter(_duplicates);
_gyro_reports->print_info("gyro queue");
::printf("checked_next: %u\n", _checked_next);
for (uint8_t i = 0; i < BMI055_GYRO_NUM_CHECKED_REGISTERS; i++) {
uint8_t v = read_reg(_checked_registers[i]);
if (v != _checked_values[i]) {
::printf("reg %02x:%02x should be %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_values[i]);
}
if (v != _checked_bad[i]) {
::printf("reg %02x:%02x was bad %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_bad[i]);
}
}
::printf("temperature: %.1f\n", (double)_last_temperature);
printf("\n");
}
void
BMI055_gyro::print_registers()
{
uint8_t index = 0;
printf("BMI055 gyro registers\n");
uint8_t reg = _checked_registers[index++];
uint8_t v = read_reg(reg);
printf("Gyro Chip Id: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Gyro Power: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Gyro Bw: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Gyro Range: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Gyro Int-en-0: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Gyro Int-en-1: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Gyro Int-Map-1: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
}
src/drivers/imu/bmi055/bmi055_main.cpp
+617
View File
@@ -0,0 +1,617 @@
#include <px4_config.h>
#include <platforms/px4_getopt.h>
#include "bmi055.hpp"
/** driver 'main' command */
extern "C" { __EXPORT int bmi055_main(int argc, char *argv[]); }
/**
* Local functions in support of the shell command.
*/
enum sensor_type {
BMI055_NONE = 0,
BMI055_ACCEL = 1,
BMI055_GYRO
};
namespace bmi055
{
BMI055_accel *g_acc_dev_int; // on internal bus (accel)
BMI055_accel *g_acc_dev_ext; // on external bus (accel)
BMI055_gyro *g_gyr_dev_int; // on internal bus (gyro)
BMI055_gyro *g_gyr_dev_ext; // on external bus (gyro)
void start(bool, enum Rotation, enum sensor_type);
void stop(bool, enum sensor_type);
void test(bool, enum sensor_type);
void reset(bool, enum sensor_type);
void info(bool, enum sensor_type);
void regdump(bool, enum sensor_type);
void testerror(bool, enum sensor_type);
void usage();
/**
* Start the driver.
*
* This function only returns if the driver is up and running
* or failed to detect the sensor.
*/
void
start(bool external_bus, enum Rotation rotation, enum sensor_type sensor)
{
int fd_acc, fd_gyr;
BMI055_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int;
const char *path_accel = external_bus ? BMI055_DEVICE_PATH_ACCEL_EXT : BMI055_DEVICE_PATH_ACCEL;
BMI055_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int;
const char *path_gyro = external_bus ? BMI055_DEVICE_PATH_GYRO_EXT : BMI055_DEVICE_PATH_GYRO;
if (sensor == BMI055_ACCEL) {
if (*g_dev_acc_ptr != nullptr)
/* if already started, the still command succeeded */
{
errx(0, "bmi055 accel sensor already started");
}
/* create the driver */
if (external_bus) {
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI)
*g_dev_acc_ptr = new BMI055_accel(PX4_SPI_BUS_EXT, path_accel, PX4_SPIDEV_EXT_BMI, rotation);
#else
errx(0, "External SPI not available");
#endif
} else {
*g_dev_acc_ptr = new BMI055_accel(PX4_SPI_BUS_SENSORS, path_accel, PX4_SPIDEV_BMI055_ACC, rotation);
}
if (*g_dev_acc_ptr == nullptr) {
goto fail_accel;
}
if (OK != (*g_dev_acc_ptr)->init()) {
goto fail_accel;
}
/* set the poll rate to default, starts automatic data collection */
fd_acc = open(path_accel, O_RDONLY);
if (fd_acc < 0) {
goto fail_accel;
}
if (ioctl(fd_acc, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail_accel;
}
close(fd_acc);
}
if (sensor == BMI055_GYRO) {
if (*g_dev_gyr_ptr != nullptr) {
errx(0, "bmi055 gyro sensor already started");
}
/* create the driver */
if (external_bus) {
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI)
*g_dev_ptr = new BMI055_gyro(PX4_SPI_BUS_EXT, path_gyro, PX4_SPIDEV_EXT_BMI, rotation);
#else
errx(0, "External SPI not available");
#endif
} else {
*g_dev_gyr_ptr = new BMI055_gyro(PX4_SPI_BUS_SENSORS, path_gyro, PX4_SPIDEV_BMI055_GYR, rotation);
}
if (*g_dev_gyr_ptr == nullptr) {
goto fail_gyro;
}
if (OK != (*g_dev_gyr_ptr)->init()) {
goto fail_gyro;
}
/* set the poll rate to default, starts automatic data collection */
fd_gyr = open(path_gyro, O_RDONLY);
if (fd_gyr < 0) {
goto fail_gyro;
}
if (ioctl(fd_gyr, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail_gyro;
}
close(fd_gyr);
}
exit(PX4_OK);
fail_accel:
if (*g_dev_acc_ptr != nullptr) {
delete (*g_dev_acc_ptr);
*g_dev_acc_ptr = nullptr;
}
PX4_WARN("No BMI055 accel found");
exit(PX4_ERROR);
fail_gyro:
if (*g_dev_gyr_ptr != nullptr) {
delete (*g_dev_gyr_ptr);
*g_dev_gyr_ptr = nullptr;
}
PX4_WARN("No BMI055 gyro found");
exit(PX4_ERROR);
}
void
stop(bool external_bus, enum sensor_type sensor)
{
BMI055_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int;
BMI055_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int;
if (sensor == BMI055_ACCEL) {
if (*g_dev_acc_ptr != nullptr) {
delete *g_dev_acc_ptr;
*g_dev_acc_ptr = nullptr;
} else {
/* warn, but not an error */
warnx("bmi055 accel sensor already stopped.");
}
}
if (sensor == BMI055_GYRO) {
if (*g_dev_gyr_ptr != nullptr) {
delete *g_dev_gyr_ptr;
*g_dev_gyr_ptr = nullptr;
} else {
/* warn, but not an error */
warnx("bmi055 gyro sensor already stopped.");
}
}
exit(0);
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test(bool external_bus, enum sensor_type sensor)
{
const char *path_accel = external_bus ? BMI055_DEVICE_PATH_ACCEL_EXT : BMI055_DEVICE_PATH_ACCEL;
const char *path_gyro = external_bus ? BMI055_DEVICE_PATH_GYRO_EXT : BMI055_DEVICE_PATH_GYRO;
accel_report a_report;
gyro_report g_report;
ssize_t sz;
if (sensor == BMI055_ACCEL) {
/* get the accel driver */
int fd_acc = open(path_accel, O_RDONLY);
if (fd_acc < 0)
err(1, "%s Accel file open failed (try 'bmi055 -A start')",
path_accel);
/* reset to manual polling */
if (ioctl(fd_acc, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) {
err(1, "accel reset to manual polling");
}
/* do a simple demand read */
sz = read(fd_acc, &a_report, sizeof(a_report));
if (sz != sizeof(a_report)) {
warnx("ret: %d, expected: %d", sz, sizeof(a_report));
err(1, "immediate accel read failed");
}
warnx("single read");
warnx("time: %lld", a_report.timestamp);
warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x);
warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y);
warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z);
warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw);
warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
(double)(a_report.range_m_s2 / BMI055_ONE_G));
warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature);
warnx("temp: \t%d\traw 0x%0x", (short)g_report.temperature_raw, (unsigned short)a_report.temperature_raw);
/* reset to default polling */
if (ioctl(fd_acc, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "accel reset to default polling");
}
close(fd_acc);
}
if (sensor == BMI055_GYRO) {
/* get the gyro driver */
int fd_gyr = open(path_gyro, O_RDONLY);
if (fd_gyr < 0) {
err(1, "%s Gyro file open failed (try 'bmi055 -G start')", path_gyro);
}
/* reset to manual polling */
if (ioctl(fd_gyr, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) {
err(1, "gyro reset to manual polling");
}
/* do a simple demand read */
sz = read(fd_gyr, &g_report, sizeof(g_report));
if (sz != sizeof(g_report)) {
warnx("ret: %d, expected: %d", sz, sizeof(g_report));
err(1, "immediate gyro read failed");
}
warnx("gyr x: \t% 9.5f\trad/s", (double)g_report.x);
warnx("gyr y: \t% 9.5f\trad/s", (double)g_report.y);
warnx("gyr z: \t% 9.5f\trad/s", (double)g_report.z);
warnx("gyr x: \t%d\traw", (int)g_report.x_raw);
warnx("gyr y: \t%d\traw", (int)g_report.y_raw);
warnx("gyr z: \t%d\traw", (int)g_report.z_raw);
warnx("gyr range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
(int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
/* reset to default polling */
if (ioctl(fd_gyr, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "gyro reset to default polling");
}
close(fd_gyr);
}
if ((sensor == BMI055_ACCEL) || (sensor == BMI055_GYRO)) {
/* XXX add poll-rate tests here too */
reset(external_bus, sensor);
}
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset(bool external_bus, enum sensor_type sensor)
{
const char *path_accel = external_bus ? BMI055_DEVICE_PATH_ACCEL_EXT : BMI055_DEVICE_PATH_ACCEL;
const char *path_gyro = external_bus ? BMI055_DEVICE_PATH_GYRO_EXT : BMI055_DEVICE_PATH_GYRO;
if (sensor == BMI055_ACCEL) {
int fd_acc = open(path_accel, O_RDONLY);
if (fd_acc < 0) {
err(1, "Opening accel file failed ");
}
if (ioctl(fd_acc, SENSORIOCRESET, 0) < 0) {
err(1, "accel driver reset failed");
}
if (ioctl(fd_acc, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "accel driver poll restart failed");
}
close(fd_acc);
}
if (sensor == BMI055_GYRO) {
int fd_gyr = open(path_gyro, O_RDONLY);
if (fd_gyr < 0) {
err(1, "Opening gyro file failed ");
}
if (ioctl(fd_gyr, SENSORIOCRESET, 0) < 0) {
err(1, "gyro driver reset failed");
}
if (ioctl(fd_gyr, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "gyro driver poll restart failed");
}
close(fd_gyr);
}
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info(bool external_bus, enum sensor_type sensor)
{
BMI055_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int;
BMI055_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int;
if (sensor == BMI055_ACCEL) {
if (*g_dev_acc_ptr == nullptr) {
errx(1, "bmi055 accel driver not running");
}
printf("state @ %p\n", *g_dev_acc_ptr);
(*g_dev_acc_ptr)->print_info();
}
if (sensor == BMI055_GYRO) {
if (*g_dev_gyr_ptr == nullptr) {
errx(1, "bmi055 gyro driver not running");
}
printf("state @ %p\n", *g_dev_gyr_ptr);
(*g_dev_gyr_ptr)->print_info();
}
exit(0);
}
/**
* Dump the register information
*/
void
regdump(bool external_bus, enum sensor_type sensor)
{
BMI055_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int;
BMI055_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int;
if (sensor == BMI055_ACCEL) {
if (*g_dev_acc_ptr == nullptr) {
errx(1, "bmi055 accel driver not running");
}
printf("regdump @ %p\n", *g_dev_acc_ptr);
(*g_dev_acc_ptr)->print_registers();
}
if (sensor == BMI055_GYRO) {
if (*g_dev_gyr_ptr == nullptr) {
errx(1, "bmi055 gyro driver not running");
}
printf("regdump @ %p\n", *g_dev_gyr_ptr);
(*g_dev_gyr_ptr)->print_registers();
}
exit(0);
}
/**
* deliberately produce an error to test recovery
*/
void
testerror(bool external_bus, enum sensor_type sensor)
{
BMI055_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int;
BMI055_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int;
if (sensor == BMI055_ACCEL) {
if (*g_dev_acc_ptr == nullptr) {
errx(1, "bmi055 accel driver not running");
}
(*g_dev_acc_ptr)->test_error();
}
if (sensor == BMI055_GYRO) {
if (*g_dev_gyr_ptr == nullptr) {
errx(1, "bmi055 gyro driver not running");
}
(*g_dev_gyr_ptr)->test_error();
}
exit(0);
}
void
usage()
{
warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'testerror'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -R rotation");
warnx(" -A (Enable Accelerometer)");
warnx(" -G (Enable Gyroscope)");
}
}//namespace ends
BMI055::BMI055(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode,
uint32_t frequency, enum Rotation rotation):
SPI(name, devname, bus, device, mode, frequency),
_whoami(0),
_call{},
_call_interval(0),
_dlpf_freq(0),
_sample_perf(perf_alloc(PC_ELAPSED, "bmi055_read")),
_bad_transfers(perf_alloc(PC_COUNT, "bmi055_bad_transfers")),
_bad_registers(perf_alloc(PC_COUNT, "bmi055_bad_registers")),
_good_transfers(perf_alloc(PC_COUNT, "bmi055_good_transfers")),
_reset_retries(perf_alloc(PC_COUNT, "bmi055_reset_retries")),
_duplicates(perf_alloc(PC_COUNT, "bmi055_duplicates")),
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
_register_wait(0),
_reset_wait(0),
_rotation(rotation),
_checked_next(0)
{
}
BMI055::~BMI055()
{
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_bad_transfers);
perf_free(_bad_registers);
perf_free(_good_transfers);
perf_free(_reset_retries);
perf_free(_duplicates);
}
uint8_t
BMI055::read_reg(unsigned reg)
{
uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
transfer(cmd, cmd, sizeof(cmd));
return cmd[1];
}
uint16_t
BMI055::read_reg16(unsigned reg)
{
uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
transfer(cmd, cmd, sizeof(cmd));
return (uint16_t)(cmd[1] << 8) | cmd[2];
}
void
BMI055::write_reg(unsigned reg, uint8_t value)
{
uint8_t cmd[2];
cmd[0] = reg | DIR_WRITE;
cmd[1] = value;
transfer(cmd, nullptr, sizeof(cmd));
}
int
bmi055_main(int argc, char *argv[])
{
bool external_bus = false;
int ch;
enum Rotation rotation = ROTATION_NONE;
enum sensor_type sensor = BMI055_NONE;
int myoptind = 1;
const char *myoptarg = NULL;
/* jump over start/off/etc and look at options first */
while ((ch = px4_getopt(argc, argv, "XR:AG", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'X':
external_bus = true;
break;
case 'R':
rotation = (enum Rotation)atoi(myoptarg);
break;
case 'A':
sensor = BMI055_ACCEL;
break;
case 'G':
sensor = BMI055_GYRO;
break;
default:
bmi055::usage();
exit(0);
}
}
const char *verb = argv[myoptind];
if (sensor == BMI055_NONE) {
bmi055::usage();
exit(0);
}
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
bmi055::start(external_bus, rotation, sensor);
}
/*
* Stop the driver.
*/
if (!strcmp(verb, "stop")) {
bmi055::stop(external_bus, sensor);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
bmi055::test(external_bus, sensor);
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset")) {
bmi055::reset(external_bus, sensor);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info")) {
bmi055::info(external_bus, sensor);
}
/*
* Print register information.
*/
if (!strcmp(verb, "regdump")) {
bmi055::regdump(external_bus, sensor);
}
if (!strcmp(verb, "testerror")) {
bmi055::testerror(external_bus, sensor);
}
bmi055::usage();
exit(1);
}
src/drivers/imu/bmi160/CMakeLists.txt
+45
View File
@@ -0,0 +1,45 @@
############################################################################
#
# Copyright (c) 2015 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.
#
############################################################################
px4_add_module(
MODULE drivers__bmi160
MAIN bmi160
STACK_MAIN 1200
COMPILE_FLAGS
SRCS
bmi160.cpp
bmi160_gyro.cpp
bmi160_main.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/drivers/imu/bmi160/bmi160.cpp
+1317
View File
File diff suppressed because it is too large Load Diff
src/drivers/imu/bmi160/bmi160.hpp
+488
View File
@@ -0,0 +1,488 @@
#ifndef BMI160_HPP_
#define BMI160_HPP_
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdlib.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <getopt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/conversions.h>
#include <nuttx/arch.h>
#include <nuttx/clock.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>
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#define BMI160_DEVICE_PATH_ACCEL "/dev/bmi160_accel"
#define BMI160_DEVICE_PATH_GYRO "/dev/bmi160_gyro"
#define BMI160_DEVICE_PATH_MAG "/dev/bmi160_mag"
#define BMI160_DEVICE_PATH_ACCEL_EXT "/dev/bmi160_accel_ext"
#define BMI160_DEVICE_PATH_GYRO_EXT "/dev/bmi160_gyro_ext"
#define BMI160_DEVICE_PATH_MAG_EXT "/dev/bmi160_mag_ext"
// BMI 160 registers
#define BMIREG_CHIP_ID 0x00
#define BMIREG_ERR_REG 0x02
#define BMIREG_PMU_STATUS 0x03
#define BMIREG_DATA_0 0x04
#define BMIREG_DATA_1 0x05
#define BMIREG_DATA_2 0x06
#define BMIREG_DATA_3 0x07
#define BMIREG_DATA_4 0x08
#define BMIREG_DATA_5 0x09
#define BMIREG_DATA_6 0x0A
#define BMIREG_DATA_7 0x0B
#define BMIREG_GYR_X_L 0x0C
#define BMIREG_GYR_X_H 0x0D
#define BMIREG_GYR_Y_L 0x0E
#define BMIREG_GYR_Y_H 0x0F
#define BMIREG_GYR_Z_L 0x10
#define BMIREG_GYR_Z_H 0x11
#define BMIREG_ACC_X_L 0x12
#define BMIREG_ACC_X_H 0x13
#define BMIREG_ACC_Y_L 0x14
#define BMIREG_ACC_Y_H 0x15
#define BMIREG_ACC_Z_L 0x16
#define BMIREG_ACC_Z_H 0x17
#define BMIREG_SENSORTIME0 0x18
#define BMIREG_SENSORTIME1 0x19
#define BMIREG_SENSORTIME2 0x1A
#define BMIREG_STATUS 0x1B
#define BMIREG_INT_STATUS_0 0x1C
#define BMIREG_INT_STATUS_1 0x1D
#define BMIREG_INT_STATUS_2 0x1E
#define BMIREG_INT_STATUS_3 0x1F
#define BMIREG_TEMP_0 0x20
#define BMIREG_TEMP_1 0x21
#define BMIREG_FIFO_LEN_0 0x22
#define BMIREG_FIFO_LEN_1 0x23
#define BMIREG_FIFO_DATA 0x24
#define BMIREG_ACC_CONF 0x40
#define BMIREG_ACC_RANGE 0x41
#define BMIREG_GYR_CONF 0x42
#define BMIREG_GYR_RANGE 0x43
#define BMIREG_MAG_CONF 0x44
#define BMIREG_FIFO_DOWNS 0x45
#define BMIREG_FIFO_CONFIG_0 0x46
#define BMIREG_FIFO_CONFIG_1 0x47
#define BMIREG_MAG_IF_0 0x4B
#define BMIREG_MAG_IF_1 0x4C
#define BMIREG_MAG_IF_2 0x4D
#define BMIREG_MAG_IF_3 0x4E
#define BMIREG_MAG_IF_4 0x4F
#define BMIREG_INT_EN_0 0x50
#define BMIREG_INT_EN_1 0x51
#define BMIREG_INT_EN_2 0x52
#define BMIREG_INT_OUT_CTRL 0x53
#define BMIREG_INT_LANTCH 0x54
#define BMIREG_INT_MAP_0 0x55
#define BMIREG_INT_MAP_1 0x56
#define BMIREG_INT_MAP_2 0x57
#define BMIREG_INT_DATA_0 0x58
#define BMIREG_INT_DATA_1 0x59
#define BMIREG_INT_LH_0 0x5A
#define BMIREG_INT_LH_1 0x5B
#define BMIREG_INT_LH_2 0x5C
#define BMIREG_INT_LH_3 0x5D
#define BMIREG_INT_LH_4 0x5E
#define BMIREG_INT_MOT_0 0x5F
#define BMIREG_INT_MOT_1 0x60
#define BMIREG_INT_MOT_2 0x61
#define BMIREG_INT_MOT_3 0x62
#define BMIREG_INT_TAP_0 0x63
#define BMIREG_INT_TAP_1 0x64
#define BMIREG_INT_ORIE_0 0x65
#define BMIREG_INT_ORIE_1 0x66
#define BMIREG_INT_FLAT_0 0x67
#define BMIREG_INT_FLAT_1 0x68
#define BMIREG_FOC_CONF 0x69
#define BMIREG_CONF 0x6A
#define BMIREG_IF_CONF 0x6B
#define BMIREG_PMU_TRIGGER 0x6C
#define BMIREG_SELF_TEST 0x6D
#define BMIREG_NV_CONF 0x70
#define BMIREG_OFFSET_ACC_X 0x71
#define BMIREG_OFFSET_ACC_Y 0x72
#define BMIREG_OFFSET_ACC_Z 0x73
#define BMIREG_OFFSET_GYR_X 0x74
#define BMIREG_OFFSET_GYR_Y 0x75
#define BMIREG_OFFSET_GYR_Z 0x76
#define BMIREG_OFFSET_EN 0x77
#define BMIREG_STEP_CONT_0 0x78
#define BMIREG_STEP_CONT_1 0x79
#define BMIREG_STEP_CONF_0 0x7A
#define BMIREG_STEP_CONF_1 0x7B
#define BMIREG_CMD 0x7E
// Configuration bits BMI 160
#define BMI160_WHO_AM_I 0xD1
//BMIREG_STATUS 0x1B
#define BMI_DRDY_ACCEL (1<<7)
#define BMI_DRDY_GYRO (1<<6)
#define BMI_DRDY_MAG (1<<5)
#define BMI_GYRO_SELF_TEST_OK (1<<1)
//BMIREG_INT_STATUS_1 0x1D
#define BMI_DRDY_INT (1<<4)
//BMIREG_ACC_CONF 0x40
#define BMI_ACCEL_RATE_25_32 (0<<3) | (0<<2) | (0<<1) | (1<<0)
#define BMI_ACCEL_RATE_25_16 (0<<3) | (0<<2) | (1<<1) | (0<<0)
#define BMI_ACCEL_RATE_25_8 (0<<3) | (0<<2) | (1<<1) | (1<<0)
#define BMI_ACCEL_RATE_25_4 (0<<3) | (1<<2) | (0<<1) | (0<<0)
#define BMI_ACCEL_RATE_25_2 (0<<3) | (1<<2) | (0<<1) | (1<<0)
#define BMI_ACCEL_RATE_25 (0<<3) | (1<<2) | (1<<1) | (0<<0)
#define BMI_ACCEL_RATE_50 (0<<3) | (1<<2) | (1<<1) | (1<<0)
#define BMI_ACCEL_RATE_100 (1<<3) | (0<<2) | (0<<1) | (0<<0)
#define BMI_ACCEL_RATE_200 (1<<3) | (0<<2) | (0<<1) | (1<<0)
#define BMI_ACCEL_RATE_400 (1<<3) | (0<<2) | (1<<1) | (0<<0)
#define BMI_ACCEL_RATE_800 (1<<3) | (0<<2) | (1<<1) | (1<<0)
#define BMI_ACCEL_RATE_1600 (1<<3) | (1<<2) | (0<<1) | (0<<0)
#define BMI_ACCEL_US (0<<7)
#define BMI_ACCEL_BWP_NORMAL (0<<6) | (1<<5) | (0<<4)
#define BMI_ACCEL_BWP_OSR2 (0<<6) | (0<<5) | (1<<4)
#define BMI_ACCEL_BWP_OSR4 (0<<6) | (0<<5) | (0<<4)
//BMIREG_ACC_RANGE 0x41
#define BMI_ACCEL_RANGE_2_G (0<<3) | (0<<2) | (1<<1) | (1<<0)
#define BMI_ACCEL_RANGE_4_G (0<<3) | (1<<2) | (0<<1) | (1<<0)
#define BMI_ACCEL_RANGE_8_G (1<<3) | (0<<2) | (0<<1) | (0<<0)
#define BMI_ACCEL_RANGE_16_G (1<<3) | (1<<2) | (0<<1) | (0<<0)
//BMIREG_GYR_CONF 0x42
#define BMI_GYRO_RATE_25 (0<<3) | (1<<2) | (1<<1) | (0<<0)
#define BMI_GYRO_RATE_50 (0<<3) | (1<<2) | (1<<1) | (1<<0)
#define BMI_GYRO_RATE_100 (1<<3) | (0<<2) | (0<<1) | (0<<0)
#define BMI_GYRO_RATE_200 (1<<3) | (0<<2) | (0<<1) | (1<<0)
#define BMI_GYRO_RATE_400 (1<<3) | (0<<2) | (1<<1) | (0<<0)
#define BMI_GYRO_RATE_800 (1<<3) | (0<<2) | (1<<1) | (1<<0)
#define BMI_GYRO_RATE_1600 (1<<3) | (1<<2) | (0<<1) | (0<<0)
#define BMI_GYRO_RATE_3200 (1<<3) | (1<<2) | (0<<1) | (1<<0)
#define BMI_GYRO_BWP_NORMAL (1<<5) | (0<<4)
#define BMI_GYRO_BWP_OSR2 (0<<5) | (1<<4)
#define BMI_GYRO_BWP_OSR4 (0<<5) | (0<<4)
//BMIREG_GYR_RANGE 0x43
#define BMI_GYRO_RANGE_2000_DPS (0<<2) | (0<<1) | (0<<0)
#define BMI_GYRO_RANGE_1000_DPS (0<<2) | (0<<1) | (1<<0)
#define BMI_GYRO_RANGE_500_DPS (0<<2) | (1<<1) | (0<<0)
#define BMI_GYRO_RANGE_250_DPS (0<<2) | (1<<1) | (1<<0)
#define BMI_GYRO_RANGE_125_DPS (1<<2) | (0<<1) | (0<<0)
//BMIREG_INT_EN_1 0x51
#define BMI_DRDY_INT_EN (1<<4)
//BMIREG_INT_OUT_CTRL 0x53
#define BMI_INT1_EN (1<<3) | (0<<2) | (1<<1) //Data Ready on INT1 High
//BMIREG_INT_MAP_1 0x56
#define BMI_DRDY_INT1 (1<<7)
//BMIREG_IF_CONF 0x6B
#define BMI_SPI_3_WIRE (1<<0)
#define BMI_SPI_4_WIRE (0<<0)
#define BMI_AUTO_DIS_SEC (0<<5) | (0<<4)
#define BMI_I2C_OIS_SEC (0<<5) | (1<<4)
#define BMI_AUTO_MAG_SEC (1<<5) | (0<<4)
//BMIREG_NV_CONF 0x70
#define BMI_SPI (1<<0)
//BMIREG_CMD 0x7E
#define BMI_ACCEL_NORMAL_MODE 0x11 //Wait at least 3.8 ms before another CMD
#define BMI_GYRO_NORMAL_MODE 0x15 //Wait at least 80 ms before another CMD
#define BMI160_SOFT_RESET 0xB6
#define BMI160_ACCEL_DEFAULT_RANGE_G 16
#define BMI160_GYRO_DEFAULT_RANGE_DPS 2000
#define BMI160_ACCEL_DEFAULT_RATE 800
#define BMI160_ACCEL_MAX_RATE 1600
#define BMI160_ACCEL_MAX_PUBLISH_RATE 280
#define BMI160_GYRO_DEFAULT_RATE 800
#define BMI160_GYRO_MAX_RATE 3200
#define BMI160_GYRO_MAX_PUBLISH_RATE BMI160_ACCEL_MAX_PUBLISH_RATE
#define BMI160_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 324
#define BMI160_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 50
#define BMI160_GYRO_DEFAULT_ONCHIP_FILTER_FREQ 254.6f
#define BMI160_GYRO_DEFAULT_DRIVER_FILTER_FREQ 50
#define BMI160_ONE_G 9.80665f
#define BMI160_BUS_SPEED 10*1000*1000
#define BMI160_TIMER_REDUCTION 200
class BMI160_gyro;
class BMI160 : public device::SPI
{
public:
BMI160(int bus, const char *path_accel, const char *path_gyro, uint32_t device, enum Rotation rotation);
virtual ~BMI160();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
void print_registers();
// deliberately cause a sensor error
void test_error();
protected:
virtual int probe();
friend class BMI160_gyro;
virtual ssize_t gyro_read(struct file *filp, char *buffer, size_t buflen);
virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg);
private:
BMI160_gyro *_gyro;
uint8_t _whoami; /** whoami result */
struct hrt_call _call;
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;
int _accel_orb_class_instance;
int _accel_class_instance;
ringbuffer::RingBuffer *_gyro_reports;
struct gyro_calibration_s _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
unsigned _dlpf_freq;
float _accel_sample_rate;
float _gyro_sample_rate;
perf_counter_t _accel_reads;
perf_counter_t _gyro_reads;
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
perf_counter_t _bad_registers;
perf_counter_t _good_transfers;
perf_counter_t _reset_retries;
perf_counter_t _duplicates;
perf_counter_t _controller_latency_perf;
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;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define BMI160_NUM_CHECKED_REGISTERS 10
static const uint8_t _checked_registers[BMI160_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[BMI160_NUM_CHECKED_REGISTERS];
uint8_t _checked_bad[BMI160_NUM_CHECKED_REGISTERS];
uint8_t _checked_next;
// last temperature reading for print_info()
float _last_temperature;
// keep last accel reading for duplicate detection
uint16_t _last_accel[3];
bool _got_duplicate;
/**
* Start automatic measurement.
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Reset chip.
*
* Resets the chip and measurements ranges, but not scale and offset.
*/
int reset();
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
*
* Called by the HRT in interrupt context at the specified rate if
* automatic polling is enabled.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void measure_trampoline(void *arg);
/**
* Fetch measurements from the sensor and update the report buffers.
*/
void measure();
/**
* Read a register from the BMI160
*
* @param The register to read.
* @return The value that was read.
*/
uint8_t read_reg(unsigned reg);
uint16_t read_reg16(unsigned reg);
/**
* Write a register in the BMI160
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_reg(unsigned reg, uint8_t value);
/**
* Modify a register in the BMI160
*
* Bits are cleared before bits are set.
*
* @param reg The register to modify.
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Write a register in the BMI160, updating _checked_values
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_checked_reg(unsigned reg, uint8_t value);
/**
* Set the BMI160 measurement range.
*
* @param max_g The maximum G value the range must support.
* @param max_dps The maximum DPS value the range must support.
* @return OK if the value can be supported, -ERANGE otherwise.
*/
int set_accel_range(unsigned max_g);
int set_gyro_range(unsigned max_dps);
/**
* Swap a 16-bit value read from the BMI160 to native byte order.
*/
uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); }
/**
* Measurement self test
*
* @return 0 on success, 1 on failure
*/
int self_test();
/**
* Accel self test
*
* @return 0 on success, 1 on failure
*/
int accel_self_test();
/**
* Gyro self test
*
* @return 0 on success, 1 on failure
*/
int gyro_self_test();
/*
set low pass filter frequency
*/
void _set_dlpf_filter(uint16_t frequency_hz);
/*
set sample rate (approximate) - 10 - 952 Hz
*/
int accel_set_sample_rate(float desired_sample_rate_hz);
int gyro_set_sample_rate(float desired_sample_rate_hz);
/*
check that key registers still have the right value
*/
void check_registers(void);
/* do not allow to copy this class due to pointer data members */
BMI160(const BMI160 &);
BMI160 operator=(const BMI160 &);
#pragma pack(push, 1)
/**
* Report conversation within the BMI160, including command byte and
* interrupt status.
*/
struct BMIReport {
uint8_t cmd;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
};
#pragma pack(pop)
};
#endif /* BMI160_HPP_ */
src/drivers/imu/bmi160/bmi160_gyro.cpp
+63
View File
@@ -0,0 +1,63 @@
#include "bmi160_gyro.hpp"
#include "bmi160.hpp"
BMI160_gyro::BMI160_gyro(BMI160 *parent, const char *path) : CDev("BMI160_gyro", path),
_parent(parent),
_gyro_topic(nullptr),
_gyro_orb_class_instance(-1),
_gyro_class_instance(-1)
{
}
BMI160_gyro::~BMI160_gyro()
{
if (_gyro_class_instance != -1) {
unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance);
}
}
int
BMI160_gyro::init()
{
int ret;
// do base class init
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
BMI160_gyro::parent_poll_notify()
{
poll_notify(POLLIN);
}
ssize_t
BMI160_gyro::read(struct file *filp, char *buffer, size_t buflen)
{
return _parent->gyro_read(filp, buffer, buflen);
}
int
BMI160_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case DEVIOCGDEVICEID:
return (int)CDev::ioctl(filp, cmd, arg);
break;
default:
return _parent->gyro_ioctl(filp, cmd, arg);
}
}
src/drivers/imu/bmi160/bmi160_gyro.hpp
+41
View File
@@ -0,0 +1,41 @@
#ifndef BMI160_GYRO_HPP_
#define BMI160_GYRO_HPP_
#include <px4_config.h>
#include "bmi160.hpp"
/**
* Helper class implementing the gyro driver node.
*/
class BMI160_gyro : public device::CDev
{
public:
BMI160_gyro(BMI160 *parent, const char *path);
~BMI160_gyro();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int init();
protected:
friend class BMI160;
void parent_poll_notify();
private:
BMI160 *_parent;
orb_advert_t _gyro_topic;
int _gyro_orb_class_instance;
int _gyro_class_instance;
/* do not allow to copy this class due to pointer data members */
BMI160_gyro(const BMI160_gyro &);
BMI160_gyro operator=(const BMI160_gyro &);
};
#endif /* BMI160_GYRO_HPP_ */
src/drivers/imu/bmi160/bmi160_main.cpp
+362
View File
@@ -0,0 +1,362 @@
#include "bmi160.hpp"
#include <board_config.h>
/** driver 'main' command */
extern "C" { __EXPORT int bmi160_main(int argc, char *argv[]); }
/**
* Local functions in support of the shell command.
*/
namespace bmi160
{
BMI160 *g_dev_int; // on internal bus
BMI160 *g_dev_ext; // on external bus
void start(bool, enum Rotation);
void stop(bool);
void test(bool);
void reset(bool);
void info(bool);
void regdump(bool);
void testerror(bool);
void usage();
/**
* Start the driver.
*
* This function only returns if the driver is up and running
* or failed to detect the sensor.
*/
void
start(bool external_bus, enum Rotation rotation)
{
int fd;
BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
const char *path_accel = external_bus ? BMI160_DEVICE_PATH_ACCEL_EXT : BMI160_DEVICE_PATH_ACCEL;
const char *path_gyro = external_bus ? BMI160_DEVICE_PATH_GYRO_EXT : BMI160_DEVICE_PATH_GYRO;
if (*g_dev_ptr != nullptr)
/* if already started, the still command succeeded */
{
errx(0, "already started");
}
/* create the driver */
if (external_bus) {
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI)
*g_dev_ptr = new BMI160(PX4_SPI_BUS_EXT, path_accel, path_gyro, PX4_SPIDEV_EXT_BMI, rotation);
#else
errx(0, "External SPI not available");
#endif
} else {
#if defined(PX4_SPIDEV_BMI)
*g_dev_ptr = new BMI160(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, PX4_SPIDEV_BMI, rotation);
#else
errx(0, "No Internal SPI CS");
#endif
}
if (*g_dev_ptr == nullptr) {
goto fail;
}
if (OK != (*g_dev_ptr)->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(path_accel, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
close(fd);
exit(0);
fail:
if (*g_dev_ptr != nullptr) {
delete (*g_dev_ptr);
*g_dev_ptr = nullptr;
}
errx(1, "driver start failed");
}
void
stop(bool external_bus)
{
BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
if (*g_dev_ptr != nullptr) {
delete *g_dev_ptr;
*g_dev_ptr = nullptr;
} else {
/* warn, but not an error */
warnx("already stopped.");
}
exit(0);
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test(bool external_bus)
{
const char *path_accel = external_bus ? BMI160_DEVICE_PATH_ACCEL_EXT : BMI160_DEVICE_PATH_ACCEL;
const char *path_gyro = external_bus ? BMI160_DEVICE_PATH_GYRO_EXT : BMI160_DEVICE_PATH_GYRO;
accel_report a_report;
gyro_report g_report;
ssize_t sz;
/* get the driver */
int fd = open(path_accel, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'bmi160 start')",
path_accel);
/* get the driver */
int fd_gyro = open(path_gyro, O_RDONLY);
if (fd_gyro < 0) {
err(1, "%s open failed", path_gyro);
}
/* reset to manual polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) {
err(1, "reset to manual polling");
}
/* do a simple demand read */
sz = read(fd, &a_report, sizeof(a_report));
if (sz != sizeof(a_report)) {
warnx("ret: %d, expected: %d", sz, sizeof(a_report));
err(1, "immediate acc read failed");
}
warnx("single read");
warnx("time: %lld", a_report.timestamp);
warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x);
warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y);
warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z);
warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw);
warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
(double)(a_report.range_m_s2 / BMI160_ONE_G));
/* do a simple demand read */
sz = read(fd_gyro, &g_report, sizeof(g_report));
if (sz != sizeof(g_report)) {
warnx("ret: %d, expected: %d", sz, sizeof(g_report));
err(1, "immediate gyro read failed");
}
warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
(int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature);
warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw);
/* reset to default polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "reset to default polling");
}
close(fd);
close(fd_gyro);
/* XXX add poll-rate tests here too */
reset(external_bus);
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset(bool external_bus)
{
const char *path_accel = external_bus ? BMI160_DEVICE_PATH_ACCEL_EXT : BMI160_DEVICE_PATH_ACCEL;
int fd = open(path_accel, 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.
*/
void
info(bool external_bus)
{
BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
if (*g_dev_ptr == nullptr) {
errx(1, "driver not running");
}
printf("state @ %p\n", *g_dev_ptr);
(*g_dev_ptr)->print_info();
exit(0);
}
/**
* Dump the register information
*/
void
regdump(bool external_bus)
{
BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
if (*g_dev_ptr == nullptr) {
errx(1, "driver not running");
}
printf("regdump @ %p\n", *g_dev_ptr);
(*g_dev_ptr)->print_registers();
exit(0);
}
/**
* deliberately produce an error to test recovery
*/
void
testerror(bool external_bus)
{
BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
if (*g_dev_ptr == nullptr) {
errx(1, "driver not running");
}
(*g_dev_ptr)->test_error();
exit(0);
}
void
usage()
{
warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'testerror'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -R rotation");
}
} // namespace
int
bmi160_main(int argc, char *argv[])
{
bool external_bus = false;
int ch;
enum Rotation rotation = ROTATION_NONE;
/* jump over start/off/etc and look at options first */
while ((ch = getopt(argc, argv, "XR:")) != EOF) {
switch (ch) {
case 'X':
external_bus = true;
break;
case 'R':
rotation = (enum Rotation)atoi(optarg);
break;
default:
bmi160::usage();
exit(0);
}
}
const char *verb = argv[optind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
bmi160::start(external_bus, rotation);
}
if (!strcmp(verb, "stop")) {
bmi160::stop(external_bus);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
bmi160::test(external_bus);
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset")) {
bmi160::reset(external_bus);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info")) {
bmi160::info(external_bus);
}
/*
* Print register information.
*/
if (!strcmp(verb, "regdump")) {
bmi160::regdump(external_bus);
}
if (!strcmp(verb, "testerror")) {
bmi160::testerror(external_bus);
}
bmi160::usage();
exit(1);
}
src/drivers/imu/fxas21002c/CMakeLists.txt
+43
View File
@@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2017 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.
#
############################################################################
px4_add_module(
MODULE drivers__fxas21002c
MAIN fxas21002c
STACK_MAIN 1200
COMPILE_FLAGS
SRCS
fxas21002c.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/drivers/imu/fxas21002c/fxas21002c.cpp
+1461
View File
File diff suppressed because it is too large Load Diff
src/drivers/imu/fxos8701cq/CMakeLists.txt
+43
View File
@@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2017 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.
#
############################################################################
px4_add_module(
MODULE drivers__fxos8701cq
MAIN fxos8701cq
STACK_MAIN 1200
COMPILE_FLAGS
SRCS
fxos8701cq.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/drivers/imu/fxos8701cq/fxos8701cq.cpp
+2066
View File
File diff suppressed because it is too large Load Diff
src/drivers/imu/l3gd20/CMakeLists.txt
+43
View File
@@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2015 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.
#
############################################################################
px4_add_module(
MODULE drivers__l3gd20
MAIN l3gd20
STACK_MAIN 1200
COMPILE_FLAGS
SRCS
l3gd20.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/drivers/imu/l3gd20/l3gd20.cpp
+1429
View File
File diff suppressed because it is too large Load Diff
src/drivers/imu/lsm303d/CMakeLists.txt
+43
View File
@@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2015 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.
#
############################################################################
px4_add_module(
MODULE drivers__lsm303d
MAIN lsm303d
STACK_MAIN 1200
COMPILE_FLAGS
SRCS
lsm303d.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/drivers/imu/lsm303d/lsm303d.cpp
+2194
View File
File diff suppressed because it is too large Load Diff
src/drivers/imu/mpu6000/CMakeLists.txt
+45
View File
@@ -0,0 +1,45 @@
############################################################################
#
# Copyright (c) 2015 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.
#
############################################################################
px4_add_module(
MODULE drivers__mpu6000
MAIN mpu6000
STACK_MAIN 1400
COMPILE_FLAGS
SRCS
mpu6000.cpp
mpu6000_i2c.cpp
mpu6000_spi.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/drivers/imu/mpu6000/mpu6000.cpp
+2725
View File
File diff suppressed because it is too large Load Diff
src/drivers/imu/mpu6000/mpu6000.h
+265
View File
@@ -0,0 +1,265 @@
/****************************************************************************
*
* Copyright (c) 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 .h
*
* Shared defines for the mpu6000 driver.
*/
#pragma once
#if defined(PX4_I2C_MPU6050_ADDR) || \
defined(PX4_I2C_MPU6000_ADDR) || \
defined(PX4_I2C_ICM_20608_G_ADDR)
# define USE_I2C
#endif
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel"
#define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro"
#define MPU_DEVICE_PATH_ACCEL1 "/dev/mpu6000_accel1"
#define MPU_DEVICE_PATH_GYRO1 "/dev/mpu6000_gyro1"
#define MPU_DEVICE_PATH_ACCEL_EXT "/dev/mpu6000_accel_ext"
#define MPU_DEVICE_PATH_GYRO_EXT "/dev/mpu6000_gyro_ext"
#define MPU_DEVICE_PATH_ACCEL_EXT1 "/dev/mpu6000_accel_ext1"
#define MPU_DEVICE_PATH_GYRO_EXT1 "/dev/mpu6000_gyro_ext1"
#define ICM20602_DEVICE_PATH_ACCEL "/dev/icm20602_accel"
#define ICM20602_DEVICE_PATH_GYRO "/dev/icm20602_gyro"
#define ICM20602_DEVICE_PATH_ACCEL1 "/dev/icm20602_accel1"
#define ICM20602_DEVICE_PATH_GYRO1 "/dev/icm20602_gyro1"
#define ICM20602_DEVICE_PATH_ACCEL_EXT "/dev/icm20602_accel_ext"
#define ICM20602_DEVICE_PATH_GYRO_EXT "/dev/icm20602_gyro_ext"
#define ICM20602_DEVICE_PATH_ACCEL_EXT1 "/dev/icm20602_accel_ext1"
#define ICM20602_DEVICE_PATH_GYRO_EXT1 "/dev/icm20602_gyro_ext1"
#define ICM20608_DEVICE_PATH_ACCEL "/dev/icm20608_accel"
#define ICM20608_DEVICE_PATH_GYRO "/dev/icm20608_gyro"
#define ICM20608_DEVICE_PATH_ACCEL1 "/dev/icm20608_accel1"
#define ICM20608_DEVICE_PATH_GYRO1 "/dev/icm20608_gyro1"
#define ICM20608_DEVICE_PATH_ACCEL_EXT "/dev/icm20608_accel_ext"
#define ICM20608_DEVICE_PATH_GYRO_EXT "/dev/icm20608_gyro_ext"
#define ICM20608_DEVICE_PATH_ACCEL_EXT1 "/dev/icm20608_accel_ext1"
#define ICM20608_DEVICE_PATH_GYRO_EXT1 "/dev/icm20608_gyro_ext1"
#define ICM20689_DEVICE_PATH_ACCEL "/dev/icm20689_accel"
#define ICM20689_DEVICE_PATH_GYRO "/dev/icm20689_gyro"
// MPU 6000 registers
#define MPUREG_WHOAMI 0x75
#define MPUREG_SMPLRT_DIV 0x19
#define MPUREG_CONFIG 0x1A
#define MPUREG_GYRO_CONFIG 0x1B
#define MPUREG_ACCEL_CONFIG 0x1C
#define MPUREG_FIFO_EN 0x23
#define MPUREG_INT_PIN_CFG 0x37
#define MPUREG_INT_ENABLE 0x38
#define MPUREG_INT_STATUS 0x3A
#define MPUREG_ACCEL_XOUT_H 0x3B
#define MPUREG_ACCEL_XOUT_L 0x3C
#define MPUREG_ACCEL_YOUT_H 0x3D
#define MPUREG_ACCEL_YOUT_L 0x3E
#define MPUREG_ACCEL_ZOUT_H 0x3F
#define MPUREG_ACCEL_ZOUT_L 0x40
#define MPUREG_TEMP_OUT_H 0x41
#define MPUREG_TEMP_OUT_L 0x42
#define MPUREG_GYRO_XOUT_H 0x43
#define MPUREG_GYRO_XOUT_L 0x44
#define MPUREG_GYRO_YOUT_H 0x45
#define MPUREG_GYRO_YOUT_L 0x46
#define MPUREG_GYRO_ZOUT_H 0x47
#define MPUREG_GYRO_ZOUT_L 0x48
#define MPUREG_USER_CTRL 0x6A
#define MPUREG_PWR_MGMT_1 0x6B
#define MPUREG_PWR_MGMT_2 0x6C
#define MPUREG_FIFO_COUNTH 0x72
#define MPUREG_FIFO_COUNTL 0x73
#define MPUREG_FIFO_R_W 0x74
#define MPUREG_PRODUCT_ID 0x0C
#define MPUREG_TRIM1 0x0D
#define MPUREG_TRIM2 0x0E
#define MPUREG_TRIM3 0x0F
#define MPUREG_TRIM4 0x10
#define MPU_GYRO_DLPF_CFG_256HZ_NOLPF2 0x00
#define MPU_GYRO_DLPF_CFG_188HZ 0x01
#define MPU_GYRO_DLPF_CFG_98HZ 0x02
#define MPU_GYRO_DLPF_CFG_42HZ 0x03
#define MPU_GYRO_DLPF_CFG_20HZ 0x04
#define MPU_GYRO_DLPF_CFG_10HZ 0x05
#define MPU_GYRO_DLPF_CFG_5HZ 0x06
#define MPU_GYRO_DLPF_CFG_2100HZ_NOLPF 0x07
#define MPU_DLPF_CFG_MASK 0x07
// Configuration bits MPU 3000 and MPU 6000 (not revised)?
#define BIT_SLEEP 0x40
#define BIT_H_RESET 0x80
#define BITS_CLKSEL 0x07
#define MPU_CLK_SEL_PLLGYROX 0x01
#define MPU_CLK_SEL_PLLGYROZ 0x03
#define MPU_EXT_SYNC_GYROX 0x02
#define BITS_GYRO_ST_X 0x80
#define BITS_GYRO_ST_Y 0x40
#define BITS_GYRO_ST_Z 0x20
#define BITS_FS_250DPS 0x00
#define BITS_FS_500DPS 0x08
#define BITS_FS_1000DPS 0x10
#define BITS_FS_2000DPS 0x18
#define BITS_FS_MASK 0x18
#define BIT_INT_ANYRD_2CLEAR 0x10
#define BIT_RAW_RDY_EN 0x01
#define BIT_I2C_IF_DIS 0x10
#define BIT_INT_STATUS_DATA 0x01
#define MPU_WHOAMI_6000 0x68
#define ICM_WHOAMI_20602 0x12
#define ICM_WHOAMI_20608 0xaf
#define ICM_WHOAMI_20689 0x98
// ICM2608 specific registers
#define ICMREG_ACCEL_CONFIG2 0x1D
#define ICM_ACC_DLPF_CFG_1046HZ_NOLPF 0x00
#define ICM_ACC_DLPF_CFG_218HZ 0x01
#define ICM_ACC_DLPF_CFG_99HZ 0x02
#define ICM_ACC_DLPF_CFG_44HZ 0x03
#define ICM_ACC_DLPF_CFG_21HZ 0x04
#define ICM_ACC_DLPF_CFG_10HZ 0x05
#define ICM_ACC_DLPF_CFG_5HZ 0x06
#define ICM_ACC_DLPF_CFG_420HZ 0x07
/* this is an undocumented register which
if set incorrectly results in getting a 2.7m/s/s offset
on the Y axis of the accelerometer
*/
#define MPUREG_ICM_UNDOC1 0x11
#define MPUREG_ICM_UNDOC1_VALUE 0xc9
// Product ID Description for ICM20602
// Read From device
#define ICM20602_REV_01 1
#define ICM20602_REV_02 2
// Product ID Description for ICM20608
#define ICM20608_REV_FF 0xff // In the past, was thought to be not returning a value. But seem repeatable.
// Product ID Description for ICM20689
#define ICM20689_REV_FE 0xfe
#define ICM20689_REV_03 0x03
// Product ID Description for MPU6000
// high 4 bits low 4 bits
// Product Name Product Revision
#define MPU6000ES_REV_C4 0x14
#define MPU6000ES_REV_C5 0x15
#define MPU6000ES_REV_D6 0x16
#define MPU6000ES_REV_D7 0x17
#define MPU6000ES_REV_D8 0x18
#define MPU6000_REV_C4 0x54
#define MPU6000_REV_C5 0x55
#define MPU6000_REV_D6 0x56
#define MPU6000_REV_D7 0x57
#define MPU6000_REV_D8 0x58
#define MPU6000_REV_D9 0x59
#define MPU6000_REV_D10 0x5A
#define MPU6050_REV_D8 0x28 // TODO:Need verification
#define MPU6000_ACCEL_DEFAULT_RANGE_G 16
#define MPU6000_ACCEL_DEFAULT_RATE 1000
#define MPU6000_ACCEL_MAX_OUTPUT_RATE 280
#define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
#define MPU6000_GYRO_DEFAULT_RANGE_G 8
#define MPU6000_GYRO_DEFAULT_RATE 1000
/* rates need to be the same between accel and gyro */
#define MPU6000_GYRO_MAX_OUTPUT_RATE MPU6000_ACCEL_MAX_OUTPUT_RATE
#define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 42
#define MPU6000_ONE_G 9.80665f
#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
#else
#define EXTERNAL_BUS 0
#endif
#define MPUIOCGIS_I2C (unsigned)(DEVIOCGDEVICEID+100)
#pragma pack(push, 1)
/**
* Report conversation within the MPU6000, including command byte and
* interrupt status.
*/
struct MPUReport {
uint8_t cmd;
uint8_t status;
uint8_t accel_x[2];
uint8_t accel_y[2];
uint8_t accel_z[2];
uint8_t temp[2];
uint8_t gyro_x[2];
uint8_t gyro_y[2];
uint8_t gyro_z[2];
};
#pragma pack(pop)
#define MPU_MAX_READ_BUFFER_SIZE (sizeof(MPUReport) + 1)
#define MPU_MAX_WRITE_BUFFER_SIZE (2)
/*
The MPU6000 can only handle high bus speeds on the sensor and
interrupt status registers. All other registers have a maximum 1MHz
Communication with all registers of the device is performed using either
I2C at 400kHz or SPI at 1MHz. For applications requiring faster communications,
the sensor and interrupt registers may be read using SPI at 20MHz
*/
#define MPU6000_LOW_BUS_SPEED 0
#define MPU6000_HIGH_BUS_SPEED 0x8000
# define MPU6000_IS_HIGH_SPEED(r) ((r) & MPU6000_HIGH_BUS_SPEED)
# define MPU6000_REG(r) ((r) &~MPU6000_HIGH_BUS_SPEED)
# define MPU6000_SET_SPEED(r, s) ((r)|(s))
# define MPU6000_HIGH_SPEED_OP(r) MPU6000_SET_SPEED((r), MPU6000_HIGH_BUS_SPEED)
# define MPU6000_LOW_SPEED_OP(r) MPU6000_REG((r))
/* interface factories */
extern device::Device *MPU6000_SPI_interface(int bus, int device_type, bool external_bus);
extern device::Device *MPU6000_I2C_interface(int bus, int device_type, bool external_bus);
extern int MPU6000_probe(device::Device *dev, int device_type);
typedef device::Device *(*MPU6000_constructor)(int, int, bool);
src/drivers/imu/mpu6000/mpu6000_i2c.cpp
+172
View File
@@ -0,0 +1,172 @@
/****************************************************************************
*
* Copyright (c) 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 mpu6000_i2c.cpp
*
* I2C interface for MPU6000 /MPU6050
*/
/* XXX trim includes */
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <assert.h>
#include <debug.h>
#include <errno.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>
#include "mpu6000.h"
#include "board_config.h"
#ifdef USE_I2C
device::Device *MPU6000_I2C_interface(int bus, int device_type, bool external_bus);
class MPU6000_I2C : public device::I2C
{
public:
MPU6000_I2C(int bus, int device_type);
virtual ~MPU6000_I2C();
virtual int init();
virtual int read(unsigned address, void *data, unsigned count);
virtual int write(unsigned address, void *data, unsigned count);
virtual int ioctl(unsigned operation, unsigned &arg);
protected:
virtual int probe();
private:
int _device_type;
};
device::Device *
MPU6000_I2C_interface(int bus, int device_type, bool external_bus)
{
return new MPU6000_I2C(bus, device_type);
}
MPU6000_I2C::MPU6000_I2C(int bus, int device_type) :
I2C("MPU6000_I2C", nullptr, bus, PX4_I2C_MPU6050_ADDR, 400000),
_device_type(device_type)
{
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000;
}
MPU6000_I2C::~MPU6000_I2C()
{
}
int
MPU6000_I2C::init()
{
/* this will call probe() */
return I2C::init();
}
int
MPU6000_I2C::ioctl(unsigned operation, unsigned &arg)
{
int ret;
switch (operation) {
case ACCELIOCGEXTERNAL:
return external();
case DEVIOCGDEVICEID:
return CDev::ioctl(nullptr, operation, arg);
case MPUIOCGIS_I2C:
return 1;
default:
ret = -EINVAL;
}
return ret;
}
int
MPU6000_I2C::write(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE];
if (sizeof(cmd) < (count + 1)) {
return -EIO;
}
cmd[0] = MPU6000_REG(reg_speed);
cmd[1] = *(uint8_t *)data;
return transfer(&cmd[0], count + 1, nullptr, 0);
}
int
MPU6000_I2C::read(unsigned reg_speed, void *data, unsigned count)
{
/* We want to avoid copying the data of MPUReport: So if the caller
* supplies a buffer not MPUReport in size, it is assume to be a reg or
* reg 16 read
* Since MPUReport has a cmd at front, we must return the data
* after that. Foe anthing else we must return it
*/
uint32_t offset = count < sizeof(MPUReport) ? 0 : offsetof(MPUReport, status);
uint8_t cmd = MPU6000_REG(reg_speed);
int ret = transfer(&cmd, 1, &((uint8_t *)data)[offset], count);
return ret == OK ? count : ret;
}
int
MPU6000_I2C::probe()
{
uint8_t whoami = 0;
uint8_t expected = _device_type == 6000 ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608;
return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
}
#endif /* PX4_I2C_OBDEV_HMC5883 */
src/drivers/imu/mpu6000/mpu6000_spi.cpp
+371
View File
@@ -0,0 +1,371 @@
/****************************************************************************
*
* 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 mpu6000_spi.cpp
*
* Driver for the Invensense MPU6000 connected via SPI.
*
* @author Andrew Tridgell
* @author Pat Hickey
* @author David sidrane
*/
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <assert.h>
#include <debug.h>
#include <errno.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/device/spi.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>
#include "mpu6000.h"
#include <board_config.h>
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#if defined(PX4_SPIDEV_MPU) || defined(PX4_SPIDEV_ICM_20602) || defined(PX4_SPIDEV_ICM_20689)
# ifdef PX4_SPI_BUS_EXT
# define EXTERNAL_BUS PX4_SPI_BUS_EXT
# else
# define EXTERNAL_BUS 0
# endif
/* The MPU6000 can only handle high SPI bus speeds of 20Mhz on the sensor and
* interrupt status registers. All other registers have a maximum 1MHz
* SPI speed
*
* The ICM parts are not rated as high.
*
* The Actual Value will be rounded down by the spi driver.
* 168 Mhz CPU 180 Mhz CPU
* Selected ------------actual---------------
* 20 Mhz 10.5 Mhz 11.250 Mhz
* 10 Mhz 5.250 Mhz 5.625 Mhz
* 8 Mhz 5.250 Mhz 5.625 Mhz
* 1 Mhz 0.703125 Mhz 0.65625 Mhz
*
*/
#define MPU6000_LOW_SPI_BUS_SPEED 1000*1000
#define MPU6000_HIGH_SPI_BUS_SPEED 20*1000*1000
#define ICM20608_HIGH_SPI_BUS_SPEED 8*1000*1000
#define ICM20689_HIGH_SPI_BUS_SPEED 8*1000*1000
#define ICM20602_HIGH_SPI_BUS_SPEED 10*1000*1000
#define UNKNOWN_HIGH_SPI_BUS_SPEED 8*1000*1000 // Use the minimum
device::Device *MPU6000_SPI_interface(int bus, int device_type, bool external_bus);
class MPU6000_SPI : public device::SPI
{
public:
MPU6000_SPI(int bus, uint32_t device, int device_type);
virtual ~MPU6000_SPI();
virtual int init();
virtual int read(unsigned address, void *data, unsigned count);
virtual int write(unsigned address, void *data, unsigned count);
virtual int ioctl(unsigned operation, unsigned &arg);
protected:
virtual int probe();
private:
int _device_type;
/* Helper to set the desired speed and isolate the register on return */
int _max_frequency;
void set_bus_frequency(unsigned &reg_speed_reg_out);
};
device::Device *
MPU6000_SPI_interface(int bus, int device_type, bool external_bus)
{
int cs = SPIDEV_NONE(0);
device::Device *interface = nullptr;
if (external_bus) {
#if defined(PX4_SPI_BUS_EXT) || defined(PX4_SPI_BUS_EXTERNAL)
switch (device_type) {
case 6000:
# if defined(PX4_SPIDEV_EXT_MPU)
cs = PX4_SPIDEV_EXT_MPU;
# endif
break;
case 20602:
# if defined(PX4_SPIDEV_ICM_20602_EXT)
cs = PX4_SPIDEV_ICM_20602_EXT;
# endif
break;
case 20608:
# if defined(PX4_SPIDEV_EXT_ICM)
cs = PX4_SPIDEV_EXT_ICM;
# elif defined(PX4_SPIDEV_ICM_20608_EXT)
cs = PX4_SPIDEV_ICM_20608_EXT;
# endif
break;
case 20689:
# if defined(PX4_SPIDEV_ICM_20689_EXT)
cs = PX4_SPIDEV_ICM_20689_EXT;
# endif
break;
default:
break;
}
#endif
} else {
switch (device_type) {
case 6000:
#if defined(PX4_SPIDEV_MPU)
cs = PX4_SPIDEV_MPU;
#endif
break;
case 20602:
#if defined(PX4_SPIDEV_ICM_20602)
cs = PX4_SPIDEV_ICM_20602;
#endif
break;
case 20608:
#if defined(PX4_SPIDEV_ICM)
cs = PX4_SPIDEV_ICM;
#elif defined(PX4_SPIDEV_ICM_20608)
cs = PX4_SPIDEV_ICM_20608;
#endif
break;
case 20689:
# if defined(PX4_SPIDEV_ICM_20689)
cs = PX4_SPIDEV_ICM_20689;
# endif
default:
break;
}
}
if (cs != SPIDEV_NONE(0)) {
interface = new MPU6000_SPI(bus, cs, device_type);
}
return interface;
}
MPU6000_SPI::MPU6000_SPI(int bus, uint32_t device, int device_type) :
SPI("MPU6000", nullptr, bus, device, SPIDEV_MODE3, MPU6000_LOW_SPI_BUS_SPEED),
_device_type(device_type)
{
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000;
}
MPU6000_SPI::~MPU6000_SPI()
{
}
int
MPU6000_SPI::init()
{
int ret;
ret = SPI::init();
if (ret != OK) {
DEVICE_DEBUG("SPI init failed");
return -EIO;
}
return OK;
}
int
MPU6000_SPI::ioctl(unsigned operation, unsigned &arg)
{
int ret;
switch (operation) {
case ACCELIOCGEXTERNAL:
external();
/* FALLTHROUGH */
case DEVIOCGDEVICEID:
return CDev::ioctl(nullptr, operation, arg);
case MPUIOCGIS_I2C:
return 0;
default: {
ret = -EINVAL;
}
}
return ret;
}
void
MPU6000_SPI::set_bus_frequency(unsigned &reg_speed)
{
/* Set the desired speed */
set_frequency(MPU6000_IS_HIGH_SPEED(reg_speed) ? _max_frequency : MPU6000_LOW_SPI_BUS_SPEED);
/* Isolate the register on return */
reg_speed = MPU6000_REG(reg_speed);
}
int
MPU6000_SPI::write(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE];
if (sizeof(cmd) < (count + 1)) {
return -EIO;
}
/* Set the desired speed and isolate the register */
set_bus_frequency(reg_speed);
cmd[0] = reg_speed | DIR_WRITE;
cmd[1] = *(uint8_t *)data;
return transfer(&cmd[0], &cmd[0], count + 1);
}
int
MPU6000_SPI::read(unsigned reg_speed, void *data, unsigned count)
{
/* We want to avoid copying the data of MPUReport: So if the caller
* supplies a buffer not MPUReport in size, it is assume to be a reg or reg 16 read
* 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 *pbuff = count < sizeof(MPUReport) ? cmd : (uint8_t *) data ;
if (count < sizeof(MPUReport)) {
/* add command */
count++;
}
set_bus_frequency(reg_speed);
/* Set command */
pbuff[0] = reg_speed | DIR_READ ;
/* Transfer the command and get the data */
int ret = transfer(pbuff, pbuff, count);
if (ret == OK && pbuff == &cmd[0]) {
/* Adjust the count back */
count--;
/* Return the data */
memcpy(data, &cmd[1], count);
}
return ret == OK ? count : ret;
}
int
MPU6000_SPI::probe()
{
uint8_t whoami = 0;
uint8_t expected = MPU_WHOAMI_6000;
_max_frequency = UNKNOWN_HIGH_SPI_BUS_SPEED;
switch (_device_type) {
default:
case 6000:
_max_frequency = MPU6000_HIGH_SPI_BUS_SPEED;
break;
case 20602:
expected = ICM_WHOAMI_20602;
_max_frequency = ICM20602_HIGH_SPI_BUS_SPEED;
break;
case 20608:
expected = ICM_WHOAMI_20608;
_max_frequency = ICM20608_HIGH_SPI_BUS_SPEED;
break;
case 20689:
expected = ICM_WHOAMI_20689;
_max_frequency = ICM20689_HIGH_SPI_BUS_SPEED;
break;
}
return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
}
#endif // PX4_SPIDEV_MPU
src/drivers/imu/mpu9250/CMakeLists.txt
+49
View File
@@ -0,0 +1,49 @@
############################################################################
#
# Copyright (c) 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.
#
############################################################################
px4_add_module(
MODULE drivers__mpu9250
MAIN mpu9250
STACK_MAIN 1200
COMPILE_FLAGS
SRCS
mpu9250.cpp
mpu9250_i2c.cpp
mpu9250_spi.cpp
main.cpp
gyro.cpp
mag.cpp
mag_i2c.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/drivers/imu/mpu9250/gyro.cpp
+131
View File
@@ -0,0 +1,131 @@
/****************************************************************************
*
* 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 <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdlib.h>
#include <errno.h>
#include <stdio.h>
#include <systemlib/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_gyro::MPU9250_gyro(MPU9250 *parent, const char *path) :
CDev("MPU9250_gyro", path),
_parent(parent),
_gyro_topic(nullptr),
_gyro_orb_class_instance(-1),
_gyro_class_instance(-1)
{
}
MPU9250_gyro::~MPU9250_gyro()
{
if (_gyro_class_instance != -1) {
unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance);
}
}
int
MPU9250_gyro::init()
{
int ret;
// do base class init
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);
}
ssize_t
MPU9250_gyro::read(struct file *filp, char *buffer, size_t buflen)
{
return _parent->gyro_read(filp, buffer, buflen);
}
int
MPU9250_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case DEVIOCGDEVICEID:
return (int)CDev::ioctl(filp, cmd, arg);
break;
default:
return _parent->gyro_ioctl(filp, cmd, arg);
}
}
src/drivers/imu/mpu9250/gyro.h
+66
View File
@@ -0,0 +1,66 @@
/****************************************************************************
*
* 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 ssize_t read(struct file *filp, char *buffer, size_t buflen);
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;
int _gyro_orb_class_instance;
int _gyro_class_instance;
/* do not allow to copy this class due to pointer data members */
MPU9250_gyro(const MPU9250_gyro &);
MPU9250_gyro operator=(const MPU9250_gyro &);
};
src/drivers/imu/mpu9250/mag.cpp
+626
View File
@@ -0,0 +1,626 @@
/****************************************************************************
*
* 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 mag.cpp
*
* Driver for the ak8963 magnetometer within the Invensense mpu9250
*
* @author Robert Dickenson
*
*/
#include <px4_config.h>
#include <px4_log.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdlib.h>
#include <errno.h>
#include <stdio.h>
#include <systemlib/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 "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),
_interface(interface),
_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{}
{
// 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;
}
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);
perf_free(_mag_overflows);
perf_free(_mag_duplicates);
}
int
MPU9250_mag::init()
{
int ret = CDev::init();
/* if cdev init failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("MPU9250 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, ORB_PRIO_LOW);
// &_mag_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
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) {
// it isn't new data - wait for next timer
return true;
} else {
memcpy(&_last_mag_data, mag_data, sizeof(_last_mag_data));
return false;
}
}
void
MPU9250_mag::measure()
{
struct ak8963_regs data;
if (OK == _interface->read(AK8963REG_ST1, &data, sizeof(struct ak8963_regs))) {
_measure(data);
}
}
void
MPU9250_mag::_measure(struct ak8963_regs data)
{
bool mag_notify = true;
if (check_duplicate((uint8_t *)&data.x) && !(data.st1 & 0x02)) {
perf_count(_mag_duplicates);
return;
}
/* monitor for if data overrun flag is ever set */
if (data.st1 & 0x02) {
perf_count(_mag_overruns);
}
/* monitor for if magnetic sensor overflow flag is ever set noting that st2
* is usually not even refreshed, but will always be in the same place in the
* mpu's buffers regardless, hence the actual count would be bogus
*/
if (data.st2 & 0x08) {
perf_count(_mag_overflows);
}
mag_report mrb;
mrb.timestamp = hrt_absolute_time();
mrb.is_external = false;
/*
* Align axes - note the accel & gryo are also re-aligned so this
* doesn't look obvious with the datasheet
*/
mrb.x_raw = data.x;
mrb.y_raw = -data.y;
mrb.z_raw = -data.z;
float xraw_f = data.x;
float yraw_f = -data.y;
float 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.range_ga = 48.0f;
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);
}
}
ssize_t
MPU9250_mag::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(mag_report);
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is not enabled, get a fresh measurement into the buffer */
if (_parent->_call_interval == 0) {
_mag_reports->flush();
/* TODO: this won't work as getting valid magnetometer
* data requires more than one measure cycle
*/
_parent->measure();
}
/* if no data, error (we could block here) */
if (_mag_reports->empty()) {
return -EAGAIN;
}
perf_count(_mag_reads);
/* copy reports out of our buffer to the caller */
mag_report *mrp = reinterpret_cast<mag_report *>(buffer);
int transferred = 0;
while (count--) {
if (!_mag_reports->get(mrp)) {
break;
}
transferred++;
mrp++;
}
/* return the number of bytes transferred */
return (transferred * sizeof(mag_report));
}
int
MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCRESET:
return ak8963_reset();
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
/*
* TODO: investigate being able to stop
* the continuous sampling
*/
//stop();
return OK;
/* external signalling not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
return ioctl(filp, SENSORIOCSPOLLRATE, 100);
case SENSOR_POLLRATE_DEFAULT:
return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_AK8963_SAMPLE_RATE);
/* adjust to a legal polling interval in Hz */
default: {
if (MPU9250_AK8963_SAMPLE_RATE != arg) {
return -EINVAL;
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
return MPU9250_AK8963_SAMPLE_RATE;
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = px4_enter_critical_section();
if (!_mag_reports->resize(arg)) {
px4_leave_critical_section(flags);
return -ENOMEM;
}
px4_leave_critical_section(flags);
return OK;
}
case MAGIOCGSAMPLERATE:
return MPU9250_AK8963_SAMPLE_RATE;
case MAGIOCSSAMPLERATE:
/*
* We don't currently support any means of changing
* the sampling rate of the mag
*/
if (MPU9250_AK8963_SAMPLE_RATE != arg) {
return -EINVAL;
}
return OK;
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;
case MAGIOCSRANGE:
return -EINVAL;
case MAGIOCGRANGE:
return 48; // fixed full scale measurement range of +/- 4800 uT == 48 Gauss
case MAGIOCSELFTEST:
return self_test();
#ifdef MAGIOCSHWLOWPASS
case MAGIOCSHWLOWPASS:
return -EINVAL;
#endif
#ifdef MAGIOCGHWLOWPASS
case MAGIOCGHWLOWPASS:
return -EINVAL;
#endif
default:
return (int)CDev::ioctl(filp, cmd, arg);
}
}
int
MPU9250_mag::self_test(void)
{
return 0;
}
void
MPU9250_mag::set_passthrough(uint8_t reg, uint8_t size, uint8_t *out)
{
uint8_t addr;
_parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // ensure slave r/w is disabled before changing the registers
if (out) {
_parent->write_reg(MPUREG_I2C_SLV0_D0, *out);
addr = AK8963_I2C_ADDR;
} else {
addr = AK8963_I2C_ADDR | BIT_I2C_READ_FLAG;
}
_parent->write_reg(MPUREG_I2C_SLV0_ADDR, addr);
_parent->write_reg(MPUREG_I2C_SLV0_REG, reg);
_parent->write_reg(MPUREG_I2C_SLV0_CTRL, size | BIT_I2C_SLV0_EN);
}
void
MPU9250_mag::read_block(uint8_t reg, uint8_t *val, uint8_t count)
{
_parent->_interface->read(reg, val, count);
}
void
MPU9250_mag::passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size)
{
set_passthrough(reg, size);
usleep(25 + 25 * size); // wait for the value to be read from slave
read_block(MPUREG_EXT_SENS_DATA_00, buf, size);
_parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // disable new reads
}
uint8_t
MPU9250_mag::read_reg(unsigned int reg)
{
uint8_t buf;
if (_interface == nullptr) {
passthrough_read(reg, &buf, 0x01);
} else {
_interface->read(reg, &buf, 1);
}
return buf;
}
bool
MPU9250_mag::ak8963_check_id(uint8_t &deviceid)
{
deviceid = read_reg(AK8963REG_WIA);
return (AK8963_DEVICE_ID == deviceid);
}
/*
* 400kHz I2C bus speed = 2.5us per bit = 25us per byte
*/
void
MPU9250_mag::passthrough_write(uint8_t reg, uint8_t val)
{
set_passthrough(reg, 1, &val);
usleep(50); // wait for the value to be written to slave
_parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // disable new writes
}
void
MPU9250_mag::write_reg(unsigned reg, uint8_t value)
{
// general register transfer at low clock speed
if (_interface == nullptr) {
passthrough_write(reg, value);
} else {
_interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1);
}
}
int
MPU9250_mag::ak8963_reset(void)
{
// First initialize it to use the bus
int rv = ak8963_setup();
if (rv == OK) {
// Now reset the mag
write_reg(AK8963REG_CNTL2, AK8963_RESET);
// Then re-initialize the bus/mag
rv = ak8963_setup();
}
return rv;
}
bool
MPU9250_mag::ak8963_read_adjustments(void)
{
uint8_t response[3];
float ak8963_ASA[3];
write_reg(AK8963REG_CNTL1, AK8963_FUZE_MODE | AK8963_16BIT_ADC);
usleep(50);
if (_interface != nullptr) {
_interface->read(AK8963REG_ASAX, response, 3);
} else {
passthrough_read(AK8963REG_ASAX, response, 3);
}
write_reg(AK8963REG_CNTL1, AK8963_POWERDOWN_MODE);
for (int i = 0; i < 3; i++) {
if (0 != response[i] && 0xff != response[i]) {
ak8963_ASA[i] = ((float)(response[i] - 128) / 256.0f) + 1.0f;
} else {
return false;
}
}
_mag_asa_x = ak8963_ASA[0];
_mag_asa_y = ak8963_ASA[1];
_mag_asa_z = ak8963_ASA[2];
return true;
}
int
MPU9250_mag::ak8963_setup_master_i2c(void)
{
/* When _interface is null we are using SPI and must
* use the parent interface to configure the device to act
* in master mode (SPI to I2C bridge)
*/
if (_interface == nullptr) {
_parent->modify_checked_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_EN);
_parent->write_reg(MPUREG_I2C_MST_CTRL, BIT_I2C_MST_P_NSR | BIT_I2C_MST_WAIT_FOR_ES | BITS_I2C_MST_CLOCK_400HZ);
} else {
_parent->modify_checked_reg(MPUREG_USER_CTRL, BIT_I2C_MST_EN, 0);
}
return OK;
}
int
MPU9250_mag::ak8963_setup(void)
{
int retries = 10;
do {
ak8963_setup_master_i2c();
write_reg(AK8963REG_CNTL2, AK8963_RESET);
uint8_t id = 0;
if (ak8963_check_id(id)) {
break;
}
retries--;
PX4_ERR("AK8963: bad id %d retries %d", id, retries);
_parent->modify_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST);
up_udelay(100);
} while (retries > 0);
if (retries > 0) {
retries = 10;
while (!ak8963_read_adjustments() && retries) {
retries--;
PX4_ERR("AK8963: failed to read adjustment data. Retries %d", retries);
_parent->modify_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST);
up_udelay(100);
ak8963_setup_master_i2c();
write_reg(AK8963REG_CNTL2, AK8963_RESET);
}
}
if (retries == 0) {
PX4_ERR("AK8963: failed to initialize, disabled!");
_parent->modify_checked_reg(MPUREG_USER_CTRL, BIT_I2C_MST_EN, 0);
_parent->write_reg(MPUREG_I2C_MST_CTRL, 0);
return -EIO;
}
write_reg(AK8963REG_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC);
if (_interface == NULL) {
/* Configure mpu' I2c Master interface to read ak8963 data
* Into to fifo
*/
set_passthrough(AK8963REG_ST1, sizeof(struct ak8963_regs));
}
return OK;
}
src/drivers/imu/mpu9250/mag.h
+159
View File
@@ -0,0 +1,159 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "drivers/device/ringbuffer.h" // ringbuffer::RingBuffer
#include "drivers/drv_mag.h" // mag_calibration_s
#include "systemlib/perf_counter.h" // perf_counter_t
/* in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */
#define 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
#define AK8963REG_WIA 0x00
#define AK8963REG_ST1 0x02
#define AK8963REG_HXL 0x03
#define AK8963REG_ASAX 0x10
#define AK8963REG_CNTL1 0x0A
#define AK8963REG_CNTL2 0x0B
#define AK8963_SINGLE_MEAS_MODE 0x01
#define AK8963_CONTINUOUS_MODE1 0x02
#define AK8963_CONTINUOUS_MODE2 0x06
#define AK8963_POWERDOWN_MODE 0x00
#define AK8963_SELFTEST_MODE 0x08
#define AK8963_FUZE_MODE 0x0F
#define AK8963_16BIT_ADC 0x10
#define AK8963_14BIT_ADC 0x00
#define AK8963_RESET 0x01
class MPU9250;
#pragma pack(push, 1)
struct ak8963_regs {
uint8_t st1;
int16_t x;
int16_t y;
int16_t z;
uint8_t st2;
};
#pragma pack(pop)
extern device::Device *AK8963_I2C_interface(int bus, bool external_bus);
typedef device::Device *(*MPU9250_mag_constructor)(int, bool);
/**
* Helper class implementing the magnetometer driver node.
*/
class MPU9250_mag : public device::CDev
{
public:
MPU9250_mag(MPU9250 *parent, device::Device *interface, const char *path);
~MPU9250_mag();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
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);
void read_block(uint8_t reg, uint8_t *val, uint8_t count);
int ak8963_reset(void);
int ak8963_setup(void);
int ak8963_setup_master_i2c(void);
bool ak8963_check_id(uint8_t &id);
bool ak8963_read_adjustments(void);
protected:
Device *_interface;
friend class MPU9250;
/* Directly measure from the _interface if possible */
void measure();
/* Update the state with prefetched data (internally called by the regular measure() )*/
void _measure(struct ak8963_regs data);
uint8_t read_reg(unsigned reg);
void write_reg(unsigned reg, uint8_t value);
bool is_passthrough() { return _interface == nullptr; }
int self_test(void);
private:
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;
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];
/* do not allow to copy this class due to pointer data members */
MPU9250_mag(const MPU9250_mag &);
MPU9250_mag operator=(const MPU9250_mag &);
};
src/drivers/imu/mpu9250/mag_i2c.cpp
+171
View File
@@ -0,0 +1,171 @@
/****************************************************************************
*
* Copyright (c) 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 mag_i2c.cpp
*
* I2C interface for AK8963
*/
/* XXX trim includes */
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <assert.h>
#include <debug.h>
#include <errno.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>
#include "mpu9250.h"
#include "mag.h"
#include "board_config.h"
#ifdef USE_I2C
device::Device *AK8963_I2C_interface(int bus, bool external_bus);
class AK8963_I2C : public device::I2C
{
public:
AK8963_I2C(int bus);
virtual ~AK8963_I2C();
virtual int init();
virtual int read(unsigned address, void *data, unsigned count);
virtual int write(unsigned address, void *data, unsigned count);
virtual int ioctl(unsigned operation, unsigned &arg);
protected:
virtual int probe();
};
device::Device *
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)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250;
}
AK8963_I2C::~AK8963_I2C()
{
}
int
AK8963_I2C::init()
{
/* this will call probe() */
return I2C::init();
}
int
AK8963_I2C::ioctl(unsigned operation, unsigned &arg)
{
int ret;
switch (operation) {
case ACCELIOCGEXTERNAL:
return external();
case DEVIOCGDEVICEID:
return CDev::ioctl(nullptr, operation, arg);
case MPUIOCGIS_I2C:
return 1;
default:
ret = -EINVAL;
}
return ret;
}
int
AK8963_I2C::write(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE];
if (sizeof(cmd) < (count + 1)) {
return -EIO;
}
cmd[0] = MPU9250_REG(reg_speed);
cmd[1] = *(uint8_t *)data;
return transfer(&cmd[0], count + 1, nullptr, 0);
}
int
AK8963_I2C::read(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd = MPU9250_REG(reg_speed);
return transfer(&cmd, 1, (uint8_t *)data, count);
}
int
AK8963_I2C::probe()
{
uint8_t whoami = 0;
uint8_t expected = AK8963_DEVICE_ID;
if (read(AK8963REG_WIA, &whoami, 1)) {
return -EIO;
}
if (whoami != expected) {
return -EIO;
}
return OK;
}
#endif
src/drivers/imu/mpu9250/main.cpp
+577
View File
@@ -0,0 +1,577 @@
/****************************************************************************
*
* 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 main.cpp
*
* Driver for the Invensense mpu9250 connected via I2C or SPI.
*
* @authors Andrew Tridgell
* Robert Dickenson
*
* based on the mpu6000 driver
*/
#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 <getopt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <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_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"
/** driver 'main' command */
extern "C" { __EXPORT int mpu9250_main(int argc, char *argv[]); }
enum MPU9250_BUS {
MPU9250_BUS_ALL = 0,
MPU9250_BUS_I2C_INTERNAL,
MPU9250_BUS_I2C_EXTERNAL,
MPU9250_BUS_SPI_INTERNAL,
MPU9250_BUS_SPI_EXTERNAL
};
/**
* Local functions in support of the shell command.
*/
namespace mpu9250
{
/*
list of supported bus configurations
*/
struct mpu9250_bus_option {
enum MPU9250_BUS busid;
const char *accelpath;
const char *gyropath;
const char *magpath;
MPU9250_constructor interface_constructor;
bool magpassthrough;
uint8_t busnum;
MPU9250 *dev;
} bus_options[] = {
#if defined (USE_I2C)
# if defined(PX4_I2C_BUS_ONBOARD)
{ MPU9250_BUS_I2C_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_I2C_interface, false, PX4_I2C_BUS_ONBOARD, NULL },
# endif
# if defined(PX4_I2C_BUS_EXPANSION)
{ 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, NULL },
# 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, NULL },
#endif
#if defined(PX4_SPI_BUS_EXT)
{ 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, NULL },
#endif
};
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
void start(enum MPU9250_BUS busid, enum Rotation rotation, bool external_bus);
bool start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external_bus);
struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid);
void stop(enum MPU9250_BUS busid);
void test(enum MPU9250_BUS busid);
void reset(enum MPU9250_BUS busid);
void info(enum MPU9250_BUS busid);
void regdump(enum MPU9250_BUS busid);
void testerror(enum MPU9250_BUS busid);
void usage();
/**
* find a bus structure for a busid
*/
struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid)
{
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
if ((busid == MPU9250_BUS_ALL ||
busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
return bus_options[i];
}
}
errx(1, "bus %u not started", (unsigned)busid);
}
/**
* start driver for a specific bus option
*/
bool
start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external)
{
int fd = -1;
if (bus.dev != nullptr) {
warnx("%s SPI not available", external ? "External" : "Internal");
return false;
}
device::Device *interface = bus.interface_constructor(bus.busnum, external);
if (interface == nullptr) {
warnx("no device on bus %u", (unsigned)bus.busid);
return false;
}
if (interface->init() != OK) {
delete interface;
warnx("no device on bus %u", (unsigned)bus.busid);
return false;
}
device::Device *mag_interface = nullptr;
#ifdef USE_I2C
/* For i2c interfaces, connect to the magnetomer directly */
bool is_i2c = bus.busid == MPU9250_BUS_I2C_INTERNAL || bus.busid == MPU9250_BUS_I2C_EXTERNAL;
if (is_i2c) {
mag_interface = AK8963_I2C_interface(bus.busnum, external);
}
#endif
bus.dev = new MPU9250(interface, mag_interface, bus.accelpath, bus.gyropath, bus.magpath, rotation);
if (bus.dev == nullptr) {
delete interface;
return false;
}
if (OK != bus.dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(bus.accelpath, O_RDONLY);
if (fd < 0) {
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;
}
errx(1, "driver start failed");
}
/**
* Start the driver.
*
* This function only returns if the driver is up and running
* or failed to detect the sensor.
*/
void
start(enum MPU9250_BUS busid, enum Rotation rotation, bool external)
{
bool started = false;
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if (busid == MPU9250_BUS_ALL && bus_options[i].dev != NULL) {
// this device is already started
continue;
}
if (busid != MPU9250_BUS_ALL && bus_options[i].busid != busid) {
// not the one that is asked for
continue;
}
started |= start_bus(bus_options[i], rotation, external);
}
exit(started ? 0 : 1);
}
void
stop(enum MPU9250_BUS busid)
{
struct mpu9250_bus_option &bus = find_bus(busid);
if (bus.dev != nullptr) {
delete bus.dev;
bus.dev = nullptr;
} else {
/* warn, but not an error */
warnx("already stopped.");
}
exit(0);
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test(enum MPU9250_BUS busid)
{
struct mpu9250_bus_option &bus = find_bus(busid);
accel_report a_report;
gyro_report g_report;
mag_report m_report;
ssize_t sz;
/* get the driver */
int fd = open(bus.accelpath, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed (try 'm start')", bus.accelpath);
}
/* get the driver */
int fd_gyro = open(bus.gyropath, O_RDONLY);
if (fd_gyro < 0) {
err(1, "%s open failed", bus.gyropath);
}
/* get the driver */
int fd_mag = open(bus.magpath, O_RDONLY);
if (fd_mag < 0) {
err(1, "%s open failed", bus.magpath);
}
/* reset to manual polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) {
err(1, "reset to manual polling");
}
/* do a simple demand read */
sz = read(fd, &a_report, sizeof(a_report));
if (sz != sizeof(a_report)) {
warnx("ret: %d, expected: %d", sz, sizeof(a_report));
err(1, "immediate acc read failed");
}
warnx("single read");
warnx("time: %lld", a_report.timestamp);
warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x);
warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y);
warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z);
warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw);
warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
(double)(a_report.range_m_s2 / MPU9250_ONE_G));
/* do a simple demand read */
sz = read(fd_gyro, &g_report, sizeof(g_report));
if (sz != sizeof(g_report)) {
warnx("ret: %d, expected: %d", sz, sizeof(g_report));
err(1, "immediate gyro read failed");
}
warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
(int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature);
warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw);
/* do a simple demand read */
sz = read(fd_mag, &m_report, sizeof(m_report));
if (sz != sizeof(m_report)) {
warnx("ret: %d, expected: %d", sz, sizeof(m_report));
err(1, "immediate mag read failed");
}
warnx("mag x: \t% 9.5f\tGa", (double)m_report.x);
warnx("mag y: \t% 9.5f\tGa", (double)m_report.y);
warnx("mag z: \t% 9.5f\tGa", (double)m_report.z);
warnx("mag x: \t%d\traw", (int)m_report.x_raw);
warnx("mag y: \t%d\traw", (int)m_report.y_raw);
warnx("mag z: \t%d\traw", (int)m_report.z_raw);
warnx("mag range: %8.4f Ga", (double)m_report.range_ga);
warnx("mag temp: %8.4f\tdeg celsius", (double)m_report.temperature);
/* reset to default polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "reset to default polling");
}
close(fd);
close(fd_gyro);
close(fd_mag);
/* XXX add poll-rate tests here too */
reset(busid);
errx(0, "PASS");
}
/**
* 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.
*/
void
info(enum MPU9250_BUS busid)
{
struct mpu9250_bus_option &bus = find_bus(busid);
if (bus.dev == nullptr) {
errx(1, "driver not running");
}
printf("state @ %p\n", bus.dev);
bus.dev->print_info();
exit(0);
}
/**
* Dump the register information
*/
void
regdump(enum MPU9250_BUS busid)
{
struct mpu9250_bus_option &bus = find_bus(busid);
if (bus.dev == nullptr) {
errx(1, "driver not running");
}
printf("regdump @ %p\n", bus.dev);
bus.dev->print_registers();
exit(0);
}
/**
* deliberately produce an error to test recovery
*/
void
testerror(enum MPU9250_BUS busid)
{
struct mpu9250_bus_option &bus = find_bus(busid);
if (bus.dev == nullptr) {
errx(1, "driver not running");
}
bus.dev->test_error();
exit(0);
}
void
usage()
{
warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'testerror'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -R rotation");
}
} // namespace
int
mpu9250_main(int argc, char *argv[])
{
enum MPU9250_BUS busid = MPU9250_BUS_ALL;
int ch;
bool external = false;
enum Rotation rotation = ROTATION_NONE;
/* jump over start/off/etc and look at options first */
while ((ch = getopt(argc, argv, "XISsR:")) != EOF) {
switch (ch) {
case 'X':
busid = MPU9250_BUS_I2C_EXTERNAL;
break;
case 'I':
busid = MPU9250_BUS_I2C_INTERNAL;
break;
case 'S':
busid = MPU9250_BUS_SPI_EXTERNAL;
break;
case 's':
busid = MPU9250_BUS_SPI_INTERNAL;
break;
case 'R':
rotation = (enum Rotation)atoi(optarg);
break;
default:
mpu9250::usage();
exit(0);
}
}
external = (busid == MPU9250_BUS_I2C_EXTERNAL || busid == MPU9250_BUS_SPI_EXTERNAL);
const char *verb = argv[optind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
mpu9250::start(busid, rotation, external);
}
if (!strcmp(verb, "stop")) {
mpu9250::stop(busid);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
mpu9250::test(busid);
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset")) {
mpu9250::reset(busid);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info")) {
mpu9250::info(busid);
}
/*
* Print register information.
*/
if (!strcmp(verb, "regdump")) {
mpu9250::regdump(busid);
}
if (!strcmp(verb, "testerror")) {
mpu9250::testerror(busid);
}
mpu9250::usage();
exit(1);
}
src/drivers/imu/mpu9250/mpu9250.cpp
+1565
View File
File diff suppressed because it is too large Load Diff
src/drivers/imu/mpu9250/mpu9250.h
+544
View File
@@ -0,0 +1,544 @@
/****************************************************************************
*
* Copyright (c) 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.
*
****************************************************************************/
#include <stdint.h>
#include <systemlib/perf_counter.h>
#include <systemlib/conversions.h>
#include <nuttx/wqueue.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/conversion/rotation.h>
#include "mag.h"
#include "gyro.h"
#if defined(PX4_I2C_OBDEV_MPU9250)
# define USE_I2C
#endif
// MPU 9250 registers
#define MPUREG_WHOAMI 0x75
#define MPUREG_SMPLRT_DIV 0x19
#define MPUREG_CONFIG 0x1A
#define MPUREG_GYRO_CONFIG 0x1B
#define MPUREG_ACCEL_CONFIG 0x1C
#define MPUREG_ACCEL_CONFIG2 0x1D
#define MPUREG_LPACCEL_ODR 0x1E
#define MPUREG_WOM_THRESH 0x1F
#define MPUREG_FIFO_EN 0x23
#define MPUREG_I2C_MST_CTRL 0x24
#define MPUREG_I2C_SLV0_ADDR 0x25
#define MPUREG_I2C_SLV0_REG 0x26
#define MPUREG_I2C_SLV0_CTRL 0x27
#define MPUREG_I2C_SLV1_ADDR 0x28
#define MPUREG_I2C_SLV1_REG 0x29
#define MPUREG_I2C_SLV1_CTRL 0x2A
#define MPUREG_I2C_SLV2_ADDR 0x2B
#define MPUREG_I2C_SLV2_REG 0x2C
#define MPUREG_I2C_SLV2_CTRL 0x2D
#define MPUREG_I2C_SLV3_ADDR 0x2E
#define MPUREG_I2C_SLV3_REG 0x2F
#define MPUREG_I2C_SLV3_CTRL 0x30
#define MPUREG_I2C_SLV4_ADDR 0x31
#define MPUREG_I2C_SLV4_REG 0x32
#define MPUREG_I2C_SLV4_DO 0x33
#define MPUREG_I2C_SLV4_CTRL 0x34
#define MPUREG_I2C_SLV4_DI 0x35
#define MPUREG_I2C_MST_STATUS 0x36
#define MPUREG_INT_PIN_CFG 0x37
#define MPUREG_INT_ENABLE 0x38
#define MPUREG_INT_STATUS 0x3A
#define MPUREG_ACCEL_XOUT_H 0x3B
#define MPUREG_ACCEL_XOUT_L 0x3C
#define MPUREG_ACCEL_YOUT_H 0x3D
#define MPUREG_ACCEL_YOUT_L 0x3E
#define MPUREG_ACCEL_ZOUT_H 0x3F
#define MPUREG_ACCEL_ZOUT_L 0x40
#define MPUREG_TEMP_OUT_H 0x41
#define MPUREG_TEMP_OUT_L 0x42
#define MPUREG_GYRO_XOUT_H 0x43
#define MPUREG_GYRO_XOUT_L 0x44
#define MPUREG_GYRO_YOUT_H 0x45
#define MPUREG_GYRO_YOUT_L 0x46
#define MPUREG_GYRO_ZOUT_H 0x47
#define MPUREG_GYRO_ZOUT_L 0x48
#define MPUREG_EXT_SENS_DATA_00 0x49
#define MPUREG_I2C_SLV0_D0 0x63
#define MPUREG_I2C_SLV1_D0 0x64
#define MPUREG_I2C_SLV2_D0 0x65
#define MPUREG_I2C_SLV3_D0 0x66
#define MPUREG_I2C_MST_DELAY_CTRL 0x67
#define MPUREG_SIGNAL_PATH_RESET 0x68
#define MPUREG_MOT_DETECT_CTRL 0x69
#define MPUREG_USER_CTRL 0x6A
#define MPUREG_PWR_MGMT_1 0x6B
#define MPUREG_PWR_MGMT_2 0x6C
#define MPUREG_FIFO_COUNTH 0x72
#define MPUREG_FIFO_COUNTL 0x73
#define MPUREG_FIFO_R_W 0x74
// Configuration bits MPU 9250
#define BIT_SLEEP 0x40
#define BIT_H_RESET 0x80
#define MPU_CLK_SEL_AUTO 0x01
#define BITS_GYRO_ST_X 0x80
#define BITS_GYRO_ST_Y 0x40
#define BITS_GYRO_ST_Z 0x20
#define BITS_FS_250DPS 0x00
#define BITS_FS_500DPS 0x08
#define BITS_FS_1000DPS 0x10
#define BITS_FS_2000DPS 0x18
#define BITS_FS_MASK 0x18
#define BITS_DLPF_CFG_250HZ 0x00
#define BITS_DLPF_CFG_184HZ 0x01
#define BITS_DLPF_CFG_92HZ 0x02
#define BITS_DLPF_CFG_41HZ 0x03
#define BITS_DLPF_CFG_20HZ 0x04
#define BITS_DLPF_CFG_10HZ 0x05
#define BITS_DLPF_CFG_5HZ 0x06
#define BITS_DLPF_CFG_3600HZ 0x07
#define BITS_DLPF_CFG_MASK 0x07
#define BITS_ACCEL_CONFIG2_41HZ 0x03
#define BIT_RAW_RDY_EN 0x01
#define BIT_INT_ANYRD_2CLEAR 0x10
#define BIT_INT_BYPASS_EN 0x02
#define BIT_I2C_READ_FLAG 0x80
#define BIT_I2C_SLV0_NACK 0x01
#define BIT_I2C_FIFO_EN 0x40
#define BIT_I2C_MST_EN 0x20
#define BIT_I2C_IF_DIS 0x10
#define BIT_FIFO_RST 0x04
#define BIT_I2C_MST_RST 0x02
#define BIT_SIG_COND_RST 0x01
#define BIT_I2C_SLV0_EN 0x80
#define BIT_I2C_SLV0_BYTE_SW 0x40
#define BIT_I2C_SLV0_REG_DIS 0x20
#define BIT_I2C_SLV0_REG_GRP 0x10
#define BIT_I2C_MST_MULT_MST_EN 0x80
#define BIT_I2C_MST_WAIT_FOR_ES 0x40
#define BIT_I2C_MST_SLV_3_FIFO_EN 0x20
#define BIT_I2C_MST_P_NSR 0x10
#define BITS_I2C_MST_CLOCK_258HZ 0x08
#define BITS_I2C_MST_CLOCK_400HZ 0x0D
#define BIT_I2C_SLV0_DLY_EN 0x01
#define BIT_I2C_SLV1_DLY_EN 0x02
#define BIT_I2C_SLV2_DLY_EN 0x04
#define BIT_I2C_SLV3_DLY_EN 0x08
#define MPU_WHOAMI_9250 0x71
#define MPU_WHOAMI_6500 0x70
#define MPU9250_ACCEL_DEFAULT_RATE 1000
#define MPU9250_ACCEL_MAX_OUTPUT_RATE 280
#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
#define MPU9250_GYRO_DEFAULT_RATE 1000
/* rates need to be the same between accel and gyro */
#define MPU9250_GYRO_MAX_OUTPUT_RATE MPU9250_ACCEL_MAX_OUTPUT_RATE
#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 41
#define MPU9250_ONE_G 9.80665f
#define MPUIOCGIS_I2C (unsigned)(DEVIOCGDEVICEID+100)
#pragma pack(push, 1)
/**
* Report conversation within the mpu, including command byte and
* interrupt status.
*/
struct MPUReport {
uint8_t cmd;
uint8_t status;
uint8_t accel_x[2];
uint8_t accel_y[2];
uint8_t accel_z[2];
uint8_t temp[2];
uint8_t gyro_x[2];
uint8_t gyro_y[2];
uint8_t gyro_z[2];
struct ak8963_regs mag;
};
#pragma pack(pop)
#define MPU_MAX_WRITE_BUFFER_SIZE (2)
/*
The MPU9250 can only handle high bus speeds on the sensor and
interrupt status registers. All other registers have a maximum 1MHz
Communication with all registers of the device is performed using either
I2C at 400kHz or SPI at 1MHz. For applications requiring faster communications,
the sensor and interrupt registers may be read using SPI at 20MHz
*/
#define MPU9250_LOW_BUS_SPEED 0
#define MPU9250_HIGH_BUS_SPEED 0x8000
# define MPU9250_IS_HIGH_SPEED(r) ((r) & MPU9250_HIGH_BUS_SPEED)
# define MPU9250_REG(r) ((r) &~MPU9250_HIGH_BUS_SPEED)
# define MPU9250_SET_SPEED(r, s) ((r)|(s))
# define MPU9250_HIGH_SPEED_OP(r) MPU9250_SET_SPEED((r), MPU9250_HIGH_BUS_SPEED)
# define MPU9250_LOW_SPEED_OP(r) MPU9250_REG((r))
/* interface factories */
extern device::Device *MPU9250_SPI_interface(int bus, bool external_bus);
extern device::Device *MPU9250_I2C_interface(int bus, bool external_bus);
extern int MPU9250_probe(device::Device *dev, int device_type);
typedef device::Device *(*MPU9250_constructor)(int, bool);
class MPU9250_mag;
class MPU9250_gyro;
class MPU9250 : public device::CDev
{
public:
MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro,
const char *path_mag,
enum Rotation rotation);
virtual ~MPU9250();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
void print_registers();
// deliberately cause a sensor error
void test_error();
protected:
Device *_interface;
virtual int probe();
friend class MPU9250_mag;
friend class MPU9250_gyro;
virtual ssize_t gyro_read(struct file *filp, char *buffer, size_t buflen);
virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg);
private:
MPU9250_gyro *_gyro;
MPU9250_mag *_mag;
uint8_t _whoami; /** whoami result */
#if defined(USE_I2C)
/*
* SPI bus based device use hrt
* I2C bus needs to use work queue
*/
work_s _work;
#endif
bool _use_hrt;
struct hrt_call _call;
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;
int _accel_orb_class_instance;
int _accel_class_instance;
ringbuffer::RingBuffer *_gyro_reports;
struct gyro_calibration_s _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
unsigned _dlpf_freq;
unsigned _sample_rate;
perf_counter_t _accel_reads;
perf_counter_t _gyro_reads;
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
perf_counter_t _bad_registers;
perf_counter_t _good_transfers;
perf_counter_t _reset_retries;
perf_counter_t _duplicates;
perf_counter_t _controller_latency_perf;
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;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define MPU9250_NUM_CHECKED_REGISTERS 11
static const uint8_t _checked_registers[MPU9250_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[MPU9250_NUM_CHECKED_REGISTERS];
uint8_t _checked_bad[MPU9250_NUM_CHECKED_REGISTERS];
uint8_t _checked_next;
// last temperature reading for print_info()
float _last_temperature;
bool check_null_data(uint32_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;
/**
* Start automatic measurement.
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Reset chip.
*
* Resets the chip and measurements ranges, but not scale and offset.
*/
int reset();
/**
* Resets the main chip (excluding the magnetometer if any).
*/
int reset_mpu();
#if defined(USE_I2C)
/**
* When the I2C interfase is on
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*
* This is the heart of the measurement state machine. This function
* alternately starts a measurement, or collects the data from the
* previous measurement.
*
* When the interval between measurements is greater than the minimum
* measurement interval, a gap is inserted between collection
* and measurement to provide the most recent measurement possible
* at the next interval.
*/
void cycle();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
void use_i2c(bool on_true) { _use_hrt = !on_true; }
#endif
bool is_i2c(void) { return !_use_hrt; }
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
*
* Called by the HRT in interrupt context at the specified rate if
* automatic polling is enabled.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void measure_trampoline(void *arg);
/**
* Fetch measurements from the sensor and update the report buffers.
*/
void measure();
/**
* Read a register from the mpu
*
* @param The register to read.
* @return The value that was read.
*/
uint8_t read_reg(unsigned reg, uint32_t speed = MPU9250_LOW_BUS_SPEED);
uint16_t read_reg16(unsigned reg);
/**
* Write a register in the mpu
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_reg(unsigned reg, uint8_t value);
/**
* Modify a register in the mpu
*
* Bits are cleared before bits are set.
*
* @param reg The register to modify.
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Write a register in the mpu, updating _checked_values
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_checked_reg(unsigned reg, uint8_t value);
/**
* Modify a checked register in the mpu
*
* Bits are cleared before bits are set.
*
* @param reg The register to modify.
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Set the mpu measurement range.
*
* @param max_g The maximum G value the range must support.
* @return OK if the value can be supported, -ERANGE otherwise.
*/
int set_accel_range(unsigned max_g);
/**
* Swap a 16-bit value read from the mpu to native byte order.
*/
uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); }
/**
* Get the internal / external state
*
* @return true if the sensor is not on the main MCU board
*/
bool is_external()
{
unsigned dummy;
return _interface->ioctl(ACCELIOCGEXTERNAL, dummy);
}
/**
* Measurement self test
*
* @return 0 on success, 1 on failure
*/
int self_test();
/**
* Accel self test
*
* @return 0 on success, 1 on failure
*/
int accel_self_test();
/**
* Gyro self test
*
* @return 0 on success, 1 on failure
*/
int gyro_self_test();
/*
set low pass filter frequency
*/
void _set_dlpf_filter(uint16_t frequency_hz);
/*
set sample rate (approximate) - 1kHz to 5Hz
*/
void _set_sample_rate(unsigned desired_sample_rate_hz);
/*
check that key registers still have the right value
*/
void check_registers(void);
/* do not allow to copy this class due to pointer data members */
MPU9250(const MPU9250 &);
MPU9250 operator=(const MPU9250 &);
};
src/drivers/imu/mpu9250/mpu9250_i2c.cpp
+167
View File
@@ -0,0 +1,167 @@
/****************************************************************************
*
* Copyright (c) 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 mpu9250_i2c.cpp
*
* I2C interface for MPU9250
*/
/* XXX trim includes */
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <assert.h>
#include <debug.h>
#include <errno.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>
#include "mpu9250.h"
#include "board_config.h"
#ifdef USE_I2C
device::Device *MPU9250_I2C_interface(int bus, bool external_bus);
class MPU9250_I2C : public device::I2C
{
public:
MPU9250_I2C(int bus);
virtual ~MPU9250_I2C();
virtual int init();
virtual int read(unsigned address, void *data, unsigned count);
virtual int write(unsigned address, void *data, unsigned count);
virtual int ioctl(unsigned operation, unsigned &arg);
protected:
virtual int probe();
};
device::Device *
MPU9250_I2C_interface(int bus, bool external_bus)
{
return new MPU9250_I2C(bus);
}
MPU9250_I2C::MPU9250_I2C(int bus) :
I2C("MPU9250_I2C", nullptr, bus, PX4_I2C_OBDEV_MPU9250, 400000)
{
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250;
}
MPU9250_I2C::~MPU9250_I2C()
{
}
int
MPU9250_I2C::init()
{
/* this will call probe() */
return I2C::init();
}
int
MPU9250_I2C::ioctl(unsigned operation, unsigned &arg)
{
int ret;
switch (operation) {
case ACCELIOCGEXTERNAL:
return external();
case DEVIOCGDEVICEID:
return CDev::ioctl(nullptr, operation, arg);
case MPUIOCGIS_I2C:
return 1;
default:
ret = -EINVAL;
}
return ret;
}
int
MPU9250_I2C::write(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE];
if (sizeof(cmd) < (count + 1)) {
return -EIO;
}
cmd[0] = MPU9250_REG(reg_speed);
cmd[1] = *(uint8_t *)data;
return transfer(&cmd[0], count + 1, nullptr, 0);
}
int
MPU9250_I2C::read(unsigned reg_speed, void *data, unsigned count)
{
/* We want to avoid copying the data of MPUReport: So if the caller
* supplies a buffer not MPUReport in size, it is assume to be a reg or
* reg 16 read
* Since MPUReport has a cmd at front, we must return the data
* after that. Foe anthing else we must return it
*/
uint32_t offset = count < sizeof(MPUReport) ? 0 : offsetof(MPUReport, status);
uint8_t cmd = MPU9250_REG(reg_speed);
return transfer(&cmd, 1, &((uint8_t *)data)[offset], count);
}
int
MPU9250_I2C::probe()
{
uint8_t whoami = 0;
uint8_t expected = MPU_WHOAMI_9250;
return (read(MPUREG_WHOAMI, &whoami, 1) == OK && (whoami == expected)) ? 0 : -EIO;
}
#endif /* USE_I2C */
src/drivers/imu/mpu9250/mpu9250_spi.cpp
+291
View File
@@ -0,0 +1,291 @@
/****************************************************************************
*
* 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 mpu9250_spi.cpp
*
* Driver for the Invensense MPU9250 connected via SPI.
*
* @author Andrew Tridgell
* @author Pat Hickey
* @author David sidrane
*/
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <assert.h>
#include <debug.h>
#include <errno.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/device/spi.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>
#include "mpu9250.h"
#include <board_config.h>
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#if PX4_SPIDEV_MPU
#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
#else
#define EXTERNAL_BUS 0
#endif
/*
* The MPU9250 can only handle high SPI bus speeds of 20Mhz on the sensor and
* interrupt status registers. All other registers have a maximum 1MHz
* SPI speed
*
* The Actual Value will be rounded down by the spi driver.
* for a 168Mhz CPU this will be 10.5 Mhz and for a 180 Mhz CPU
* it will be 11.250 Mhz
*/
#define MPU9250_LOW_SPI_BUS_SPEED 1000*1000
#define MPU9250_HIGH_SPI_BUS_SPEED 20*1000*1000
device::Device *MPU9250_SPI_interface(int bus, bool external_bus);
class MPU9250_SPI : public device::SPI
{
public:
MPU9250_SPI(int bus, uint32_t device);
virtual ~MPU9250_SPI();
virtual int init();
virtual int read(unsigned address, void *data, unsigned count);
virtual int write(unsigned address, void *data, unsigned count);
virtual int ioctl(unsigned operation, unsigned &arg);
protected:
virtual int probe();
private:
/* Helper to set the desired speed and isolate the register on return */
void set_bus_frequency(unsigned &reg_speed_reg_out);
};
device::Device *
MPU9250_SPI_interface(int bus, bool external_bus)
{
uint32_t cs = SPIDEV_NONE(0);
device::Device *interface = nullptr;
if (external_bus) {
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_MPU)
cs = PX4_SPIDEV_EXT_MPU;
#else
errx(0, "External SPI not available");
#endif
} else {
cs = PX4_SPIDEV_MPU;
}
if (cs != SPIDEV_NONE(0)) {
interface = new MPU9250_SPI(bus, cs);
}
return interface;
}
MPU9250_SPI::MPU9250_SPI(int bus, uint32_t device) :
SPI("MPU9250", nullptr, bus, device, SPIDEV_MODE3, MPU9250_LOW_SPI_BUS_SPEED)
{
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250;
}
MPU9250_SPI::~MPU9250_SPI()
{
}
int
MPU9250_SPI::init()
{
int ret;
ret = SPI::init();
if (ret != OK) {
DEVICE_DEBUG("SPI init failed");
return -EIO;
}
return OK;
}
int
MPU9250_SPI::ioctl(unsigned operation, unsigned &arg)
{
int ret;
switch (operation) {
case ACCELIOCGEXTERNAL:
external();
/* FALLTHROUGH */
case DEVIOCGDEVICEID:
return CDev::ioctl(nullptr, operation, arg);
case MPUIOCGIS_I2C:
return 0;
default: {
ret = -EINVAL;
}
}
return ret;
}
void
MPU9250_SPI::set_bus_frequency(unsigned &reg_speed)
{
/* Set the desired speed */
set_frequency(MPU9250_IS_HIGH_SPEED(reg_speed) ? MPU9250_HIGH_SPI_BUS_SPEED : MPU9250_LOW_SPI_BUS_SPEED);
/* Isoolate the register on return */
reg_speed = MPU9250_REG(reg_speed);
}
int
MPU9250_SPI::write(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE];
if (sizeof(cmd) < (count + 1)) {
return -EIO;
}
/* Set the desired speed and isolate the register */
set_bus_frequency(reg_speed);
cmd[0] = reg_speed | DIR_WRITE;
cmd[1] = *(uint8_t *)data;
return transfer(&cmd[0], &cmd[0], count + 1);
}
int
MPU9250_SPI::read(unsigned reg_speed, void *data, unsigned count)
{
/* We want to avoid copying the data of MPUReport: So if the caller
* supplies a buffer not MPUReport in size, it is assume to be a reg or reg 16 read
* 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 *pbuff = count < sizeof(MPUReport) ? cmd : (uint8_t *) data ;
if (count < sizeof(MPUReport)) {
/* add command */
count++;
}
set_bus_frequency(reg_speed);
/* Set command */
pbuff[0] = reg_speed | DIR_READ ;
/* Transfer the command and get the data */
int ret = transfer(pbuff, pbuff, count);
if (ret == OK && pbuff == &cmd[0]) {
/* Adjust the count back */
count--;
/* Return the data */
memcpy(data, &cmd[1], count);
}
return ret;
}
int
MPU9250_SPI::probe()
{
uint8_t whoami = 0;
int ret = read(MPUREG_WHOAMI, &whoami, 1);
if (ret != OK) {
return -EIO;
}
switch (whoami) {
case MPU_WHOAMI_9250:
case MPU_WHOAMI_6500:
ret = 0;
break;
default:
PX4_WARN("probe failed! %u", whoami);
ret = -EIO;
}
return ret;
}
#endif // PX4_SPIDEV_MPU