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

Significant work on mpu9250 driver to get magnetometer updates happening reliably. Improved 'mag' utility to aid driver development and testing.

Browse Source
This commit is contained in:
Robert Dickenson
2016-03-08 09:29:17 +01:00
committed by Lorenz Meier
parent 505f331c36
commit 5903891213
11 changed files with 896 additions and 892 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/mpu9250/CMakeLists.txt
+1 -1
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
# 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
src/drivers/mpu9250/gyro.cpp
+1
View File
@@ -65,6 +65,7 @@
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/conversion/rotation.h>
#include "mag.h"
#include "gyro.h"
#include "mpu9250.h"
src/drivers/mpu9250/gyro.h
+6 -6
View File
@@ -39,8 +39,8 @@ class MPU9250;
class MPU9250_gyro : public device::CDev
{
public:
MPU9250_gyro(MPU9250 *parent, const char *path);
~MPU9250_gyro();
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);
@@ -48,17 +48,17 @@ public:
virtual int init();
protected:
friend class MPU9250;
friend class MPU9250;
void parent_poll_notify();
private:
MPU9250 *_parent;
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 &);
MPU9250_gyro(const MPU9250_gyro &);
MPU9250_gyro operator=(const MPU9250_gyro &);
};
src/drivers/mpu9250/mag.cpp
Regular → Executable
+514 -158
View File
@@ -33,6 +33,11 @@
/**
* @file mag.cpp
*
* Driver for the ak8963 magnetometer within the Invensense mpu9250.
*
* @author Robert Dickenson
*
*/
#include <px4_config.h>
@@ -62,13 +67,86 @@
#include "mag.h"
#include "mpu9250.h"
////////////////////////////////////////////////////////////////////////////////
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#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_EXT_SENS_DATA_00 0x49
#define MPUREG_I2C_SLV0_D0 0x63
#define MPUREG_I2C_MST_DELAY_CTRL 0x67
#define MPUREG_USER_CTRL 0x6A
#define BIT_I2C_MST_P_NSR 0x10
#define BIT_I2C_MST_EN 0x20
#define BITS_I2C_MST_CLOCK_400HZ 0x0D
//#define BITS_I2C_MST_DLY_1KHZ 0x09
#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 BIT_I2C_SLVO_EN 0x80
#define BIT_I2C_READ_FLAG 0x80
#define AK8963_I2C_ADDR 0x0C
#define AK8963_WIA 0x00
#define AK8963_ST1 0x02
#define AK8963_HXL 0x03
#define AK8963_DEVICE_ID 0x48
#define AK8963_CNTL1 0x0A
#define AK8963_SINGLE_MEAS_MODE 0x01
#define AK8963_CONTINUOUS_MODE1 0x02
#define AK8963_CONTINUOUS_MODE2 0x06
#define AK8963_SELFTEST_MODE 0x08
#define AK8963_POWERDOWN_MODE 0x00
#define AK8963_FUZE_MODE 0x0F
#define AK8963_16BIT_ADC 0x10
#define AK8963_14BIT_ADC 0x00
#define AK8963_CNTL2 0x0B
#define AK8963_RESET 0x01
#define AK8963_ASAX 0x10
#define AK8963_HXL 0x03
////////////////////////////////////////////////////////////////////////////////
float ak8963_ASA[3] = { 0, 0, 0 }; // TODO, make member variable
MPU9250_mag::MPU9250_mag(MPU9250 *parent, const char *path) :
CDev("MPU9250_mag", path),
_parent(parent),
_mag_topic(nullptr),
_mag_orb_class_instance(-1),
_mag_class_instance(-1)
_mag_class_instance(-1),
_mag_reading_data(false),
_mag_call_interval(0),
_mag_reports(nullptr),
_mag_scale{},
_mag_range_scale(0.0f),
_mag_sample_rate(1000),
_mag_reads(perf_alloc(PC_COUNT, "mpu9250_mag_read"))
{
// 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;
}
MPU9250_mag::~MPU9250_mag()
@@ -76,6 +154,10 @@ 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;
}
perf_free(_mag_reads);
}
int
@@ -91,185 +173,459 @@ MPU9250_mag::init()
return ret;
}
_mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr) {
goto out;
}
_mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
/* Initialize offsets and scales */
_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;
ak8963_setup();
// measure(0);
/* 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) {
warnx("ADVERT FAIL");
}
out:
printf("MPU9250_mag::init() completed\n");
return ret;
}
uint8_t cnt0 = 0;
uint8_t cnt1 = 0;
uint8_t cnt2 = 0;
uint8_t cnt3 = 0;
uint8_t cnt4 = 0;
/*
struct ak8963_regs {
uint8_t id;
uint8_t info;
uint8_t st1;
int16_t x;
int16_t y;
int16_t z;
uint8_t st2;
};
*/
void
MPU9250_mag::parent_poll_notify()
MPU9250_mag::measure(struct ak8963_regs data)
{
poll_notify(POLLIN);
bool mag_notify = true;
if (data.st1 & 0x01) {
if (false == _mag_reading_data) {
cnt1++;
_parent->write_reg(MPUREG_I2C_SLV0_CTRL, BIT_I2C_SLVO_EN | sizeof(struct ak8963_regs));
_mag_reading_data = true;
return;
} else {
cnt2++;
_parent->write_reg(MPUREG_I2C_SLV0_CTRL, BIT_I2C_SLVO_EN | 1);
_mag_reading_data = false;
}
} else {
if (true == _mag_reading_data) {
cnt4++;
_parent->write_reg(MPUREG_I2C_SLV0_CTRL, BIT_I2C_SLVO_EN | 1);
_mag_reading_data = false;
} else {
cnt3++;
}
return;
}
mag_report mrb;
mrb.timestamp = hrt_absolute_time();
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);
/*
struct sensor_mag_s {
uint64_t timestamp;
uint64_t error_count;
float x;
float y;
float z;
float range_ga;
float scaling;
float temperature;
int16_t x_raw;
int16_t y_raw;
int16_t z_raw;
};
*/
_mag_range_scale = 0.15e-3f;
mrb.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
mrb.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
mrb.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
mrb.range_ga = (float)48.0;
mrb.scaling = _mag_range_scale;
mrb.temperature = _parent->_last_temperature;
static uint64_t prev_mag_timestamp = 0;
mrb.error_count = ((uint64_t)(mrb.timestamp - prev_mag_timestamp) << 40)
+ ((uint64_t)cnt0 << 32)
+ ((uint32_t)cnt1 << 24) + ((uint32_t)cnt2 << 16)
+ ((uint32_t)cnt3 << 8) + cnt4;
cnt0 = cnt1 = cnt2 = cnt3 = cnt4 = 0;
prev_mag_timestamp = mrb.timestamp;
_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)
{
return _parent->mag_read(filp, buffer, buflen);
printf("MPU9250_mag::read(..)\n");
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 (_mag_call_interval == 0) {
_mag_reports->flush();
_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 DEVIOCGDEVICEID:
return (int)CDev::ioctl(filp, cmd, arg);
break;
case SENSORIOCRESET:
// return reset();
return _parent->ioctl(filp, cmd, arg);
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
// stop();
_mag_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, 100);
#define MPU9250_AK8963_DEFAULT_RATE 100
#define MPU9250_AK8963_TIMER_REDUCTION 0
case SENSOR_POLLRATE_DEFAULT:
return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_AK8963_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
// bool want_start = (_mag_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... */
_mag_call_interval = ticks;
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_mag_call_interval == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return 1000000 / _mag_call_interval;
case SENSORIOCSQUEUEDEPTH: {
printf("MPU9250_mag::ioctl(.. %l) SENSORIOCSQUEUEDEPTH\n", arg);
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_mag_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _mag_reports->size();
case MAGIOCGSAMPLERATE:
printf("MPU9250_mag::ioctl() MAGIOCGSAMPLERATE\n");
return _mag_sample_rate;
case MAGIOCSSAMPLERATE:
{
/* convert hz to hrt interval via microseconds */
// unsigned ticks = 1000000 / arg;
/* check against maximum sane rate */
// if (ticks < 1000) {
// return -EINVAL;
// }
uint8_t div = 1000 / arg;
if (div > 200) { div = 200; }
if (div < 1) { div = 1; }
// write_checked_reg(MPUREG_SMPLRT_DIV, div - 1);
_mag_sample_rate = 1000 / div;
printf("MPU9250_mag::ioctl MAGIOCSSAMPLERATE %u\n", _mag_sample_rate);
}
return OK;
/*
case MAGIOCGLOWPASS:
return _mag_filter_x.get_cutoff_freq();
case MAGIOCSLOWPASS:
// set software filtering
_mag_filter_x.set_cutoff_frequency(1.0e6f / _mag_call_interval, arg);
_mag_filter_y.set_cutoff_frequency(1.0e6f / _mag_call_interval, arg);
_mag_filter_z.set_cutoff_frequency(1.0e6f / _mag_call_interval, arg);
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:
printf("MPU9250_mag::ioctl() MAGIOCSELFTEST\n");
return self_test();
#ifdef MAGIOCSHWLOWPASS
case MAGIOCSHWLOWPASS:
return -EINVAL;
#endif
#ifdef MAGIOCGHWLOWPASS
case MAGIOCGHWLOWPASS:
return -EINVAL;
#endif
// case DEVIOCGDEVICEID:
// return (int)CDev::ioctl(filp, cmd, arg);
// break;
default:
return _parent->mag_ioctl(filp, cmd, arg);
return (int)CDev::ioctl(filp, cmd, arg);
}
}
#if 0
int
HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
MPU9250_mag::self_test(void)
{
unsigned dummy = arg;
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) 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: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return 1000000 / TICK2USEC(_measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
return reset();
case MAGIOCSSAMPLERATE:
/* same as pollrate because device is in single measurement mode*/
return ioctl(filp, SENSORIOCSPOLLRATE, arg);
case MAGIOCGSAMPLERATE:
/* same as pollrate because device is in single measurement mode*/
return 1000000 / TICK2USEC(_measure_ticks);
case MAGIOCSRANGE:
return set_range(arg);
case MAGIOCGRANGE:
return _range_ga;
case MAGIOCSLOWPASS:
case MAGIOCGLOWPASS:
/* not supported, no internal filtering */
return -EINVAL;
case MAGIOCSSCALE:
/* set new scale factors */
memcpy(&_scale, (mag_scale *)arg, sizeof(_scale));
/* check calibration, but not actually return an error */
(void)check_calibration();
return 0;
case MAGIOCGSCALE:
/* copy out scale factors */
memcpy((mag_scale *)arg, &_scale, sizeof(_scale));
return 0;
case MAGIOCCALIBRATE:
return calibrate(filp, arg);
case MAGIOCEXSTRAP:
return set_excitement(arg);
case MAGIOCSELFTEST:
return check_calibration();
case MAGIOCGEXTERNAL:
DEVICE_DEBUG("MAGIOCGEXTERNAL in main driver");
return _interface->ioctl(cmd, dummy);
case MAGIOCSTEMPCOMP:
return set_temperature_compensation(arg);
case DEVIOCGDEVICEID:
return _interface->ioctl(cmd, dummy);
default:
/* give it to the superclass */
return CDev::ioctl(filp, cmd, arg);
}
return 1;
}
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_SLVO_EN);
}
void
MPU9250_mag::read_block(uint8_t reg, uint8_t *val, uint8_t count)
{
uint8_t addr = reg | 0x80;
uint8_t tx[32] = { addr, };
uint8_t rx[32];
_parent->transfer(tx, rx, count + 1);
memcpy(val, rx + 1, 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
}
bool
MPU9250_mag::ak8963_check_id(void)
{
for (int i = 0; i < 5; i++) {
uint8_t deviceid = 0;
passthrough_read(AK8963_WIA, &deviceid, 0x01);
if (deviceid == AK8963_DEVICE_ID) {
// printf("ak8963_check_id: %02x\n", deviceid);
return true;
}
}
return false;
}
/*
* 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::ak8963_read(void)
{
/* struct ak8963_regs data;
memset(&data, 0, sizeof(struct ak8963_regs));
passthrough_read(AK8963_WIA, (uint8_t*)&data, sizeof(struct ak8963_regs));
if (data.id == 0x48) {
printf("magxyz [%02x]:", data.st2);
printf(" %d %d %d\n", data.x, data.y, data.z);
} else {
printf("invalid ak8963 read\n");
}
*/
}
void
MPU9250_mag::ak8963_reset(void)
{
passthrough_write(AK8963_CNTL2, AK8963_RESET);
}
bool
MPU9250_mag::ak8963_setup(void)
{
// enable the I2C master to slaves on the aux bus
uint8_t user_ctrl = _parent->read_reg(MPUREG_USER_CTRL);
_parent->write_checked_reg(MPUREG_USER_CTRL, user_ctrl | BIT_I2C_MST_EN);
_parent->write_reg(MPUREG_I2C_MST_CTRL, BIT_I2C_MST_P_NSR | BITS_I2C_MST_CLOCK_400HZ);
// _parent->write_reg(MPUREG_I2C_MST_DELAY_CTRL, BIT_I2C_SLV0_DLY_EN);
if (!ak8963_check_id()) {
printf("AK8963: bad id\n");
}
uint8_t response[3];
passthrough_write(AK8963_CNTL1, AK8963_FUZE_MODE | AK8963_16BIT_ADC);
passthrough_read(AK8963_ASAX, response, 3);
for (int i = 0; i < 3; i++) {
float data = response[i];
ak8963_ASA[i] = ((data - 128) / 256 + 1);
printf("AK8963_calibrate %d: %i, %f\n", i, response[i], (double)ak8963_ASA[i]);
}
// passthrough_write(AK8963_CNTL1, AK8963_CONTINUOUS_MODE1 | AK8963_16BIT_ADC);
passthrough_write(AK8963_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC);
// passthrough_write(AK8963_CNTL1, AK8963_SINGLE_MEAS_MODE | AK8963_16BIT_ADC);
set_passthrough(AK8963_ST1, 1);
return true;
}
#endif
src/drivers/mpu9250/mag.h
+40 -14
View File
@@ -33,33 +33,59 @@
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)
/**
* Helper class implementing the magnetometer driver node.
*/
class MPU9250_mag : public device::CDev
{
public:
MPU9250_mag(MPU9250 *parent, const char *path);
~MPU9250_mag();
MPU9250_mag(MPU9250 *parent, 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 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();
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);
void ak8963_read(void);
void ak8963_reset(void);
bool ak8963_setup(void);
bool ak8963_check_id(void);
protected:
friend class MPU9250;
friend class MPU9250;
void parent_poll_notify();
void measure(struct ak8963_regs data);
int self_test(void);
private:
MPU9250 *_parent;
orb_advert_t _mag_topic;
int _mag_orb_class_instance;
int _mag_class_instance;
MPU9250 *_parent;
orb_advert_t _mag_topic;
int _mag_orb_class_instance;
int _mag_class_instance;
bool _mag_reading_data;
unsigned _mag_call_interval;
ringbuffer::RingBuffer *_mag_reports;
struct mag_scale _mag_scale;
float _mag_range_scale;
unsigned _mag_sample_rate;
perf_counter_t _mag_reads;
/* do not allow to copy this class due to pointer data members */
MPU9250_mag(const MPU9250_mag &);
MPU9250_mag operator=(const MPU9250_mag &);
MPU9250_mag(const MPU9250_mag &);
MPU9250_mag operator=(const MPU9250_mag &);
};
src/drivers/mpu9250/main.cpp
+56 -81
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
* 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
@@ -36,7 +36,8 @@
*
* Driver for the Invensense mpu9250 connected via SPI.
*
* @author Andrew Tridgell
* @authors Andrew Tridgell
* Robert Dickenson
*
* based on the mpu6000 driver
*/
@@ -77,6 +78,7 @@
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/conversion/rotation.h>
#include "mag.h"
#include "gyro.h"
#include "mpu9250.h"
@@ -120,13 +122,13 @@ void
start(bool external_bus, enum Rotation rotation)
{
int fd;
MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL;
const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO;
const char *path_mag = external_bus ? MPU_DEVICE_PATH_MAG_EXT : MPU_DEVICE_PATH_MAG;
const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO;
const char *path_mag = external_bus ? MPU_DEVICE_PATH_MAG_EXT : MPU_DEVICE_PATH_MAG;
if (*g_dev_ptr != nullptr)
/* if already started, the still command succeeded */
/* if already started, the still command succeeded */
{
errx(0, "already started");
}
@@ -134,13 +136,13 @@ start(bool external_bus, enum Rotation rotation)
/* create the driver */
if (external_bus) {
#ifdef PX4_SPI_BUS_EXT
*g_dev_ptr = new MPU9250(PX4_SPI_BUS_EXT, path_accel, path_gyro, path_mag, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation);
*g_dev_ptr = new MPU9250(PX4_SPI_BUS_EXT, path_accel, path_gyro, path_mag, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation);
#else
errx(0, "External SPI not available");
#endif
} else {
*g_dev_ptr = new MPU9250(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, path_mag, (spi_dev_e)PX4_SPIDEV_MPU, rotation);
*g_dev_ptr = new MPU9250(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, path_mag, (spi_dev_e)PX4_SPIDEV_MPU, rotation);
}
if (*g_dev_ptr == nullptr) {
@@ -178,7 +180,7 @@ fail:
void
stop(bool external_bus)
{
MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
if (*g_dev_ptr != nullptr) {
delete *g_dev_ptr;
@@ -201,35 +203,34 @@ void
test(bool external_bus)
{
const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL;
const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO;
const char *path_mag = external_bus ? MPU_DEVICE_PATH_MAG_EXT : MPU_DEVICE_PATH_MAG;
accel_report a_report;
gyro_report g_report;
mag_report m_report;
ssize_t sz;
const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO;
const char *path_mag = external_bus ? MPU_DEVICE_PATH_MAG_EXT : MPU_DEVICE_PATH_MAG;
accel_report a_report;
gyro_report g_report;
mag_report m_report;
ssize_t sz;
/* get the driver */
int fd = open(path_accel, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'm start')",
path_accel);
err(1, "%s open failed (try 'm start')", path_accel);
/* get the driver */
int fd_gyro = open(path_gyro, O_RDONLY);
/* get the driver */
int fd_gyro = open(path_gyro, O_RDONLY);
if (fd_gyro < 0) {
err(1, "%s open failed", path_gyro);
}
if (fd_gyro < 0) {
err(1, "%s open failed", path_gyro);
}
/* get the driver */
int fd_mag = open(path_mag, O_RDONLY);
/* get the driver */
int fd_mag = open(path_mag, O_RDONLY);
if (fd_mag < 0) {
err(1, "%s open failed", path_mag);
}
if (fd_mag < 0) {
err(1, "%s open failed", path_mag);
}
/* reset to manual polling */
/* reset to manual polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) {
err(1, "reset to manual polling");
}
@@ -273,31 +274,31 @@ test(bool external_bus)
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));
/* 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");
}
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\trad/s", (double)m_report.x);
warnx("mag y: \t% 9.5f\trad/s", (double)m_report.y);
warnx("mag z: \t% 9.5f\trad/s", (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 rad/s (%d deg/s)", (double)m_report.range_rad_s,
// (int)((m_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
warnx("mag x: \t% 9.5f\trad/s", (double)m_report.x);
warnx("mag y: \t% 9.5f\trad/s", (double)m_report.y);
warnx("mag z: \t% 9.5f\trad/s", (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 rad/s (%d deg/s)", (double)m_report.range_rad_s,
// (int)((m_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
/* reset to default polling */
/* 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);
close(fd_gyro);
close(fd_mag);
/* XXX add poll-rate tests here too */
@@ -343,7 +344,6 @@ info(bool external_bus)
errx(1, "driver not running");
}
printf("state @ %p\n", *g_dev_ptr);
(*g_dev_ptr)->print_info();
exit(0);
@@ -361,7 +361,6 @@ regdump(bool external_bus)
errx(1, "driver not running");
}
printf("regdump @ %p\n", *g_dev_ptr);
(*g_dev_ptr)->print_registers();
exit(0);
@@ -384,18 +383,6 @@ testerror(bool external_bus)
exit(0);
}
void
w(bool external_bus)
{
MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
if (*g_dev_ptr == nullptr) {
errx(1, "driver not running");
}
(*g_dev_ptr)->ak8963_read();
exit(0);
}
void
usage()
{
@@ -407,9 +394,6 @@ usage()
} // namespace
extern int mpu_report_valid_mag;
extern int mpu_report_invalid_mag;
int
mpu9250_main(int argc, char *argv[])
{
@@ -440,54 +424,45 @@ mpu9250_main(int argc, char *argv[])
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
mpu9250::start(external_bus, rotation);
mpu9250::start(external_bus, rotation);
}
if (!strcmp(verb, "stop")) {
mpu9250::stop(external_bus);
mpu9250::stop(external_bus);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
mpu9250::test(external_bus);
mpu9250::test(external_bus);
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset")) {
mpu9250::reset(external_bus);
mpu9250::reset(external_bus);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info")) {
mpu9250::info(external_bus);
mpu9250::info(external_bus);
}
/*
* Print register information.
*/
if (!strcmp(verb, "regdump")) {
mpu9250::regdump(external_bus);
mpu9250::regdump(external_bus);
}
if (!strcmp(verb, "testerror")) {
mpu9250::testerror(external_bus);
}
if (!strcmp(verb, "testerror")) {
mpu9250::testerror(external_bus);
}
if (!strcmp(verb, "w")) {
mpu9250::w(external_bus);
}
if (!strcmp(verb, "g")) {
printf("valid %u invalid %u\n", mpu_report_valid_mag, mpu_report_invalid_mag);
exit(0);
}
mpu9250::usage();
mpu9250::usage();
exit(1);
}
src/drivers/mpu9250/mpu9250.cpp
+72 -498
View File
@@ -84,8 +84,6 @@
#define DIR_READ 0x80
#define DIR_WRITE 0x00
// MPU 9250 registers
#define MPUREG_WHOAMI 0x75
#define MPUREG_SMPLRT_DIV 0x19
@@ -201,11 +199,6 @@
#define MPU9250_TIMER_REDUCTION 200
int mpu_report_valid_mag = 0;
int mpu_report_invalid_mag = 0;
/*
list of registers that will be checked in check_registers(). Note
that MPUREG_PRODUCT_ID must be first in the list.
@@ -242,15 +235,10 @@ MPU9250::MPU9250(int bus, const char *path_accel, const char *path_gyro, const c
_gyro_scale{},
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_mag_reports(nullptr),
_mag_scale{},
_mag_range_scale(0.0f),
_mag_range_rad_s(0.0f),
_dlpf_freq(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ),
_sample_rate(1000),
_accel_reads(perf_alloc(PC_COUNT, "mpu9250_accel_read")),
_gyro_reads(perf_alloc(PC_COUNT, "mpu9250_gyro_read")),
_mag_reads(perf_alloc(PC_COUNT, "mpu9250_mag_read")),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu9250_read")),
_bad_transfers(perf_alloc(PC_COUNT, "mpu9250_bad_transfers")),
_bad_registers(perf_alloc(PC_COUNT, "mpu9250_bad_registers")),
@@ -266,18 +254,14 @@ MPU9250::MPU9250(int bus, const char *path_accel, const char *path_gyro, const c
_gyro_filter_x(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_y(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_z(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
// _mag_filter_x(MPU9250_MAG_DEFAULT_RATE, MPU9250_MAG_DEFAULT_DRIVER_FILTER_FREQ),
// _mag_filter_y(MPU9250_MAG_DEFAULT_RATE, MPU9250_MAG_DEFAULT_DRIVER_FILTER_FREQ),
// _mag_filter_z(MPU9250_MAG_DEFAULT_RATE, MPU9250_MAG_DEFAULT_DRIVER_FILTER_FREQ),
_accel_int(1000000 / MPU9250_ACCEL_MAX_OUTPUT_RATE),
_gyro_int(1000000 / MPU9250_GYRO_MAX_OUTPUT_RATE, true),
// _mag_int(1000000 / MPU9250_MAG_MAX_OUTPUT_RATE, true),
_rotation(rotation),
_checked_next(0),
_last_temperature(0),
_last_accel{},
_got_duplicate(false),
_got_duplicate_mag(false)
// _last_accel{},
_last_accel_data{},
_got_duplicate(false)
{
// disable debug() calls
_debug_enabled = false;
@@ -308,14 +292,6 @@ MPU9250::MPU9250(int bus, const char *path_accel, const char *path_gyro, const c
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
// 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;
memset(&_call, 0, sizeof(_call));
}
@@ -339,10 +315,6 @@ MPU9250::~MPU9250()
delete _gyro_reports;
}
if (_mag_reports != nullptr) {
delete _mag_reports;
}
if (_accel_class_instance != -1) {
unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance);
}
@@ -351,7 +323,6 @@ MPU9250::~MPU9250()
perf_free(_sample_perf);
perf_free(_accel_reads);
perf_free(_gyro_reads);
perf_free(_mag_reads);
perf_free(_bad_transfers);
perf_free(_bad_registers);
perf_free(_good_transfers);
@@ -392,12 +363,6 @@ MPU9250::init()
goto out;
}
_mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr) {
goto out;
}
if (reset() != OK) {
goto out;
}
@@ -417,17 +382,6 @@ MPU9250::init()
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
_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;
ak8963_setup();
/* do CDev init for the gyro device node, keep it optional */
ret = _gyro->init();
@@ -437,6 +391,15 @@ MPU9250::init()
return ret;
}
/* do CDev init for the gyro device node, keep it optional */
ret = _mag->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("mag init failed");
return ret;
}
_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
measure();
@@ -453,7 +416,6 @@ MPU9250::init()
warnx("ADVERT FAIL");
}
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_gyro_reports->get(&grp);
@@ -465,21 +427,7 @@ MPU9250::init()
warnx("ADVERT FAIL");
}
/* advertise sensor topic, measure manually to initialize valid report */
struct mag_report mrp;
_mag_reports->get(&mrp);
_mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
&_mag->_mag_orb_class_instance, ORB_PRIO_LOW);
// &_mag->_mag_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_mag->_mag_topic == nullptr) {
warnx("ADVERT FAIL");
}
out:
printf("MPU9250::init() completed\n");
return ret;
}
@@ -782,15 +730,6 @@ MPU9250::gyro_self_test()
return 0;
}
int
MPU9250::mag_self_test()
{
if (self_test()) {
return 1;
}
return 0;
}
/*
deliberately trigger an error in the sensor to trigger recovery
*/
@@ -908,7 +847,7 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg)
_call_interval = ticks;
/*
set call interval faster then the sample time. We
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 mpu9250 clock
@@ -999,19 +938,16 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg)
return accel_self_test();
#ifdef ACCELIOCSHWLOWPASS
case ACCELIOCSHWLOWPASS:
_set_dlpf_filter(arg);
return OK;
#endif
#ifdef ACCELIOCGHWLOWPASS
case ACCELIOCGHWLOWPASS:
return _dlpf_freq;
#endif
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
@@ -1091,14 +1027,12 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
return gyro_self_test();
#ifdef GYROIOCSHWLOWPASS
case GYROIOCSHWLOWPASS:
_set_dlpf_filter(arg);
return OK;
#endif
#ifdef GYROIOCGHWLOWPASS
case GYROIOCGHWLOWPASS:
return _dlpf_freq;
#endif
@@ -1109,139 +1043,6 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
}
}
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 (_call_interval == 0) {
_mag_reports->flush();
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) {
/* these are shared with the accel side */
case SENSORIOCSPOLLRATE:
case SENSORIOCGPOLLRATE:
case SENSORIOCRESET:
return ioctl(filp, cmd, arg);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_mag_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _mag_reports->size();
case MAGIOCGSAMPLERATE:
printf("MAGIOCGSAMPLERATE\n");
return _sample_rate;
case MAGIOCSSAMPLERATE:
printf("MAGIOCSSAMPLERATE\n");
// _set_sample_rate(arg); // todo: RobD
return OK;
/*
case MAGIOCGLOWPASS:
return _mag_filter_x.get_cutoff_freq();
case MAGIOCSLOWPASS:
// set software filtering
_mag_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_mag_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_mag_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
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:
/* XXX not implemented */
// XXX change these two values on set:
// _mag_range_scale = xx
// _mag_range_rad_s = xx
return -EINVAL;
case MAGIOCGRANGE:
return (unsigned long)(_mag_range_rad_s * 180.0f / M_PI_F + 0.5f);
case MAGIOCSELFTEST:
printf("MAGIOCSELFTEST\n");
return mag_self_test();
#ifdef MAGIOCSHWLOWPASS
case MAGIOCSHWLOWPASS:
// _set_dlpf_filter(arg);
return OK;
#endif
#ifdef MAGIOCGHWLOWPASS
case MAGIOCGHWLOWPASS:
return _dlpf_freq;
#endif
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
}
}
uint8_t
MPU9250::read_reg(unsigned reg, uint32_t speed)
{
@@ -1350,6 +1151,11 @@ MPU9250::start()
/* discard any stale data in the buffers */
_accel_reports->flush();
_gyro_reports->flush();
_mag->_mag_reports->flush();
// void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg);
printf("MPU9250::start() - polling at %u\n", _call_interval - MPU9250_TIMER_REDUCTION);
/* start polling at the specified rate */
hrt_call_every(&_call,
@@ -1432,6 +1238,46 @@ MPU9250::check_registers(void)
_checked_next = (_checked_next + 1) % MPU9250_NUM_CHECKED_REGISTERS;
}
bool MPU9250::check_null_data(uint32_t* data, uint8_t size)
{
while (size--) {
if (*data++) {
perf_count(_good_transfers);
return false;
}
}
// 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 mpu6k does go bad it would cause a FMU failure,
// regardless of whether another sensor is available,
return true;
}
bool MPU9250::check_duplicate(uint8_t* accel_data)
{
/*
see if this is duplicate accelerometer data. Note that we
can't use the data ready interrupt status bit in the status
register as that also goes high on new gyro data, and when
we run with BITS_DLPF_CFG_256HZ_NOLPF2 the gyro is being
sampled at 8kHz, so we would incorrectly think we have new
data when we are in fact getting duplicate accelerometer data.
*/
if (!_got_duplicate && memcmp(accel_data, &_last_accel_data, sizeof(_last_accel_data)) == 0) {
// it isn't new data - wait for next timer
perf_end(_sample_perf);
perf_count(_duplicates);
_got_duplicate = true;
} else {
memcpy(&_last_accel_data, accel_data, sizeof(_last_accel_data));
_got_duplicate = false;
}
return _got_duplicate;
}
void
MPU9250::measure()
{
@@ -1450,9 +1296,6 @@ MPU9250::measure()
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
int16_t mag_x;
int16_t mag_y;
int16_t mag_z;
} report;
/* start measuring */
@@ -1472,62 +1315,32 @@ MPU9250::measure()
check_registers();
/*
see if this is duplicate accelerometer data. Note that we
can't use the data ready interrupt status bit in the status
register as that also goes high on new gyro data, and when
we run with BITS_DLPF_CFG_256HZ_NOLPF2 the gyro is being
sampled at 8kHz, so we would incorrectly think we have new
data when we are in fact getting duplicate accelerometer data.
*/
if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) {
// it isn't new data - wait for next timer
perf_end(_sample_perf);
perf_count(_duplicates);
_got_duplicate = true;
if (check_duplicate(&mpu_report.accel_x[0])) {
extern uint8_t cnt0;
cnt0++;
return;
}
memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6);
_got_duplicate = false;
_mag->measure(mpu_report.mag);
/*
* Convert from big to little endian
*/
report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
report.temp = int16_t_from_bytes(mpu_report.temp);
report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x);
report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
report.temp = int16_t_from_bytes(mpu_report.temp);
report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x);
report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
if (report.accel_x == 0 &&
report.accel_y == 0 &&
report.accel_z == 0 &&
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 mpu6k does go bad it would cause a FMU failure,
// regardless of whether another sensor is available,
if (check_null_data((uint32_t*)&report, sizeof(report) / 4)) {
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
// 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;
}
@@ -1554,12 +1367,11 @@ MPU9250::measure()
*/
accel_report arb;
gyro_report grb;
mag_report mrb;
/*
* Adjust and scale results to m/s^2.
*/
mrb.timestamp = grb.timestamp = arb.timestamp = hrt_absolute_time();
grb.timestamp = arb.timestamp = hrt_absolute_time();
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
@@ -1582,7 +1394,6 @@ MPU9250::measure()
* 74 from all measurements centers them around zero.
*/
/* NOTE: Axes have been swapped to match the board a few lines above. */
arb.x_raw = report.accel_x;
@@ -1612,7 +1423,7 @@ MPU9250::measure()
arb.y_integral = aval_integrated(1);
arb.z_integral = aval_integrated(2);
arb.scaling = _accel_range_scale;
arb.scaling = _accel_range_scale;
arb.range_m_s2 = _accel_range_m_s2;
_last_temperature = (report.temp) / 361.0f + 35.0f;
@@ -1620,8 +1431,6 @@ MPU9250::measure()
arb.temperature_raw = report.temp;
arb.temperature = _last_temperature;
////////////////////////////////////////////////////////////////////////////////
grb.x_raw = report.gyro_x;
grb.y_raw = report.gyro_y;
grb.z_raw = report.gyro_z;
@@ -1655,85 +1464,8 @@ MPU9250::measure()
grb.temperature_raw = report.temp;
grb.temperature = _last_temperature;
///////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
bool mag_notify = false;
if (mpu_report.mag.id == 0x48) {
if (!_got_duplicate_mag && memcmp(&mpu_report.mag.x, &_last_mag[0], 6) == 0) {
// it isn't new data - wait for next timer
// perf_end(_sample_perf);
// perf_count(_duplicates);
_got_duplicate_mag = true;
// return;
} else {
memcpy(&_last_mag[0], &mpu_report.mag.x, 6);
_got_duplicate_mag = false;
mag_notify = true;
}
mpu_report_valid_mag++;
} else {
mpu_report_invalid_mag++;
}
static int mag_period = 0;
if (mag_period++ > 10) {
set_passthrough(0, sizeof(struct ak8963_regs));
mag_period = 0;
}
// static int mag_period_show = 0;
// if (mag_period_show++ > 1000) {
// mag_period_show = 0;
// printf("magxyz %d %d %d\n", mpu_report.mag.x, mpu_report.mag.y, mpu_report.mag.z);
// }
///////////////////////////////////////////////////////////////////////////////////////////////////
report.mag_x = mpu_report.mag.x;
report.mag_y = mpu_report.mag.y;
report.mag_z = mpu_report.mag.z;
mrb.x_raw = report.mag_x;
mrb.y_raw = report.mag_y;
mrb.z_raw = report.mag_z;
xraw_f = report.mag_x;
yraw_f = report.mag_y;
zraw_f = report.mag_z;
////////////////////////////
/* apply user specified rotation */
// rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
/*
struct sensor_mag_s {
uint64_t timestamp;
uint64_t error_count;
float x;
float y;
float z;
float range_ga;
float scaling;
float temperature;
int16_t x_raw;
int16_t y_raw;
int16_t z_raw;
};
*/
mrb.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
mrb.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
mrb.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
mrb.range_ga = (float)8.0;
// mrb.range_ga = (float)_mag_range_ga;
mrb.scaling = _mag_range_scale;
mrb.temperature = _last_temperature;
// mrb.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
mrb.error_count = mpu_report_invalid_mag;
///////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
_accel_reports->force(&arb);
_gyro_reports->force(&grb);
_mag_reports->force(&mrb);
/* notify anyone waiting for data */
if (accel_notify) {
@@ -1744,10 +1476,6 @@ struct sensor_mag_s {
_gyro->parent_poll_notify();
}
if (mag_notify) {
_mag->parent_poll_notify();
}
if (accel_notify && !(_pub_blocked)) {
/* log the time of this report */
perf_begin(_controller_latency_perf);
@@ -1760,11 +1488,6 @@ struct sensor_mag_s {
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
if (mag_notify && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mrb);
}
/* stop measuring */
perf_end(_sample_perf);
}
@@ -1782,7 +1505,7 @@ MPU9250::print_info()
perf_print_counter(_duplicates);
_accel_reports->print_info("accel queue");
_gyro_reports->print_info("gyro queue");
_mag_reports->print_info("mag queue");
_mag->_mag_reports->print_info("mag queue");
::printf("checked_next: %u\n", _checked_next);
for (uint8_t i = 0; i < MPU9250_NUM_CHECKED_REGISTERS; i++) {
@@ -1822,152 +1545,3 @@ MPU9250::print_registers()
printf("\n");
}
////////////////////////////////////////////////////////////////////////////////
#define BIT_I2C_SLVO_EN 0x80
#define BIT_I2C_READ_FLAG 0x80
#define AK8963_I2C_ADDR 0x0C
#define AK8963_WIA 0x00
#define AK8963_DEVICE_ID 0x48
#define AK8963_CNTL1 0x0A
#define AK8963_CONTINUOUS_MODE1 0x02
#define AK8963_CONTINUOUS_MODE2 0x06
#define AK8963_SELFTEST_MODE 0x08
#define AK8963_POWERDOWN_MODE 0x00
#define AK8963_FUZE_MODE 0x0F
#define AK8963_16BIT_ADC 0x10
#define AK8963_14BIT_ADC 0x00
#define AK8963_CNTL2 0x0B
#define AK8963_RESET 0x01
#define AK8963_ASAX 0x10
#define AK8963_HXL 0x03
#define BIT_I2C_MST_P_NSR 0x10
#define BIT_I2C_MST_EN 0x20
#define BITS_I2C_MST_CLOCK_400HZ 0x0D
#define BITS_I2C_MST_DLY_1KHZ 0x09
#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
float ak8963_ASA[3] = { 0, 0, 0 };
void
MPU9250::set_passthrough(uint8_t reg, uint8_t size, uint8_t *out)
{
uint8_t addr;
write_reg(MPUREG_I2C_SLV0_CTRL, 0); // ensure slave r/w is disabled before changing the registers
if (out) {
write_reg(MPUREG_I2C_SLV0_D0, *out);
addr = AK8963_I2C_ADDR;
} else {
addr = AK8963_I2C_ADDR | BIT_I2C_READ_FLAG;
}
write_reg(MPUREG_I2C_SLV0_ADDR, addr);
write_reg(MPUREG_I2C_SLV0_REG, reg);
write_reg(MPUREG_I2C_SLV0_CTRL, size | BIT_I2C_SLVO_EN);
}
void
MPU9250::read_block(uint8_t reg, uint8_t *val, uint8_t count)
{
uint8_t addr = reg | 0x80;
uint8_t tx[32] = { addr, };
uint8_t rx[32];
transfer(tx, rx, count + 1);
memcpy(val, rx + 1, count);
}
void
MPU9250::passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size)
{
set_passthrough(reg, size);
// usleep(20000); // wait for the value to be read from slave
usleep(50000); // wait for the value to be read from slave
read_block(MPUREG_EXT_SENS_DATA_00, buf, size);
write_reg(MPUREG_I2C_SLV0_CTRL, 0); // disable new reads
}
bool
MPU9250::ak8963_check_id(void)
{
for (int i = 0; i < 5; i++) {
uint8_t deviceid = 0;
passthrough_read(AK8963_WIA, &deviceid, 0x01);
if (deviceid == AK8963_DEVICE_ID) {
printf("ak8963_check_id: %02x\n", deviceid);
return true;
}
}
return false;
}
void
MPU9250::passthrough_write(uint8_t reg, uint8_t val)
{
set_passthrough(reg, 1, &val);
// usleep(10000); // wait for the value to be written to slave
usleep(100000); // wait for the value to be written to slave
write_reg(MPUREG_I2C_SLV0_CTRL, 0); // disable new writes
}
void
MPU9250::ak8963_read(void)
{
struct ak8963_regs data;
memset(&data, 0, sizeof(struct ak8963_regs));
passthrough_read(AK8963_WIA, (uint8_t*)&data, sizeof(struct ak8963_regs));
if (data.id == 0x48) {
printf("magxyz [%02x]:", data.st2);
printf(" %d %d %d\n", data.x, data.y, data.z);
} else {
printf("invalid ak8963 read\n");
}
}
void
MPU9250::ak8963_reset(void)
{
passthrough_write(AK8963_CNTL2, AK8963_RESET);
}
bool
MPU9250::ak8963_setup(void)
{
// enable the I2C master to slaves on the aux bus
uint8_t user_ctrl = read_reg(MPUREG_USER_CTRL);
write_checked_reg(MPUREG_USER_CTRL, user_ctrl | BIT_I2C_MST_EN);
write_reg(MPUREG_I2C_MST_CTRL, BIT_I2C_MST_P_NSR | BITS_I2C_MST_CLOCK_400HZ);
write_reg(MPUREG_I2C_SLV0_CTRL, BITS_I2C_MST_DLY_1KHZ);
write_reg(MPUREG_I2C_MST_DELAY_CTRL, BIT_I2C_SLV0_DLY_EN);
if (!ak8963_check_id()) {
printf("AK8963: wrong id\n");
}
uint8_t response[3];
passthrough_write(AK8963_CNTL1, AK8963_FUZE_MODE | AK8963_16BIT_ADC);
passthrough_read(AK8963_ASAX, response, 3);
for (int i = 0; i < 3; i++) {
float data = response[i];
ak8963_ASA[i] = ((data - 128) / 256 + 1);
printf("ak8963_calibrate %d: %i, %f\n", i, response[i], (double)ak8963_ASA[i]);
}
passthrough_write(AK8963_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC);
return true;
}
src/drivers/mpu9250/mpu9250.h
+47 -94
View File
@@ -20,8 +20,8 @@ class MPU9250_gyro;
class MPU9250 : public device::SPI
{
public:
MPU9250(int bus, const char *path_accel, const char *path_gyro, const char *path_mag, spi_dev_e device, enum Rotation rotation);
virtual ~MPU9250();
MPU9250(int bus, const char *path_accel, const char *path_gyro, const char *path_mag, spi_dev_e device, enum Rotation rotation);
virtual ~MPU9250();
virtual int init();
@@ -36,35 +36,21 @@ public:
void print_registers();
// deliberately cause a sensor error
void test_error();
// begin experimenting with the internal magnetometer
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);
void ak8963_read(void);
void ak8963_reset(void);
bool ak8963_setup(void);
bool ak8963_check_id(void);
void test_error();
protected:
virtual int probe();
friend class MPU9250_mag;
friend class MPU9250_gyro;
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);
virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen);
virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg);
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 */
MPU9250_gyro *_gyro;
MPU9250_mag *_mag;
uint8_t _whoami; /** whoami result */
struct hrt_call _call;
unsigned _call_interval;
@@ -78,25 +64,18 @@ private:
int _accel_orb_class_instance;
int _accel_class_instance;
ringbuffer::RingBuffer *_gyro_reports;
ringbuffer::RingBuffer *_gyro_reports;
struct gyro_scale _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
struct gyro_scale _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
ringbuffer::RingBuffer *_mag_reports;
struct mag_scale _mag_scale;
float _mag_range_scale;
float _mag_range_rad_s;
unsigned _dlpf_freq;
unsigned _dlpf_freq;
unsigned _sample_rate;
perf_counter_t _accel_reads;
perf_counter_t _gyro_reads;
perf_counter_t _mag_reads;
perf_counter_t _sample_perf;
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;
@@ -110,16 +89,12 @@ private:
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;
// math::LowPassFilter2p _mag_filter_x;
// math::LowPassFilter2p _mag_filter_y;
// math::LowPassFilter2p _mag_filter_z;
math::LowPassFilter2p _gyro_filter_x;
math::LowPassFilter2p _gyro_filter_y;
math::LowPassFilter2p _gyro_filter_z;
Integrator _accel_int;
Integrator _gyro_int;
// Integrator _mag_int;
Integrator _gyro_int;
enum Rotation _rotation;
@@ -135,13 +110,12 @@ private:
// 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
uint16_t _last_accel[3];
bool _got_duplicate;
// keep last magetometer reading for duplicate detection
uint16_t _last_mag[3];
bool _got_duplicate_mag;
uint8_t _last_accel_data[6];
// uint16_t _last_accel[3];
bool _got_duplicate;
/**
* Start automatic measurement.
@@ -177,7 +151,7 @@ private:
void measure();
/**
* Read a register from the mpu
* Read a register from the mpu
*
* @param The register to read.
* @return The value that was read.
@@ -186,7 +160,7 @@ private:
uint16_t read_reg16(unsigned reg);
/**
* Write a register in the mpu
* Write a register in the mpu
*
* @param reg The register to write.
* @param value The new value to write.
@@ -194,7 +168,7 @@ private:
void write_reg(unsigned reg, uint8_t value);
/**
* Modify a register in the mpu
* Modify a register in the mpu
*
* Bits are cleared before bits are set.
*
@@ -205,7 +179,7 @@ private:
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Write a register in the mpu, updating _checked_values
* Write a register in the mpu, updating _checked_values
*
* @param reg The register to write.
* @param value The new value to write.
@@ -213,7 +187,7 @@ private:
void write_checked_reg(unsigned reg, uint8_t value);
/**
* Set the mpu measurement range.
* 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.
@@ -221,7 +195,7 @@ private:
int set_accel_range(unsigned max_g);
/**
* Swap a 16-bit value read from the mpu to native byte order.
* Swap a 16-bit value read from the mpu to native byte order.
*/
uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); }
@@ -246,21 +220,14 @@ private:
*/
int accel_self_test();
/**
* Gyro self test
*
* @return 0 on success, 1 on failure
*/
int gyro_self_test();
/**
* Gyro self test
*
* @return 0 on success, 1 on failure
*/
int gyro_self_test();
/**
* Magnetometer self test
*
* @return 0 on success, 1 on failure
*/
int mag_self_test();
/*
/*
set low pass filter frequency
*/
void _set_dlpf_filter(uint16_t frequency_hz);
@@ -276,22 +243,12 @@ private:
void check_registers(void);
/* do not allow to copy this class due to pointer data members */
MPU9250(const MPU9250 &);
MPU9250 operator=(const MPU9250 &);
MPU9250(const MPU9250 &);
MPU9250 operator=(const MPU9250 &);
#pragma pack(push, 1)
struct ak8963_regs {
uint8_t id;
uint8_t info;
uint8_t st1;
int16_t x;
int16_t y;
int16_t z;
uint8_t st2;
};
/**
* Report conversation within the mpu, including command byte and
* Report conversation within the mpu, including command byte and
* interrupt status.
*/
struct MPUReport {
@@ -301,14 +258,10 @@ private:
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];
// uint8_t mag_x[2];
// uint8_t mag_y[2];
// uint8_t mag_z[2];
struct ak8963_regs mag;
};
uint8_t gyro_x[2];
uint8_t gyro_y[2];
uint8_t gyro_z[2];
struct ak8963_regs mag;
};
#pragma pack(pop)
};
src/systemcmds/mag/CMakeLists.txt
+1 -1
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
# 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
src/systemcmds/mag/mag.c
+157 -38
View File
@@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Andrew Tridgell
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
* Author: Robert Dickenson
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -34,6 +34,12 @@
/**
* @file mag.c
*
* Utility for viewing magnetometer ORB publications, specifically for
* development work on the Invensense mpu9250 connected via SPI.
*
* @author Robert Dickenson
*
*/
#include <px4_config.h>
@@ -68,23 +74,17 @@ struct sensor_mag_s {
};
*/
__EXPORT int mag_main(int argc, char *argv[]);
int mag1(void);
int mag2(void);
int
mag_main(int argc, char *argv[])
mag1(void)
{
if (argc < 2) {
printf("Usage: mag [update rate 1 - 10 hertz]\n");
exit(1);
}
int rate = atoi(argv[1]);
printf("mag %u\n", rate);
if (rate > 10 || rate < 1) {
rate = 1;
}
int16_t x_raw = 0;
int16_t y_raw = 0;
int16_t z_raw = 0;
struct mag_report mrb;
int mag_fd = orb_subscribe(ORB_ID(sensor_mag));
struct mag_report mrp;
while (true) {
@@ -92,35 +92,154 @@ mag_main(int argc, char *argv[])
orb_check(mag_fd, &updated);
if (updated) {
orb_copy(ORB_ID(sensor_mag), mag_fd, &mrp);
orb_copy(ORB_ID(sensor_mag), mag_fd, &mrb);
printf("magxyz %d %d %d\n", mrp.x_raw, mrp.y_raw, mrp.z_raw);
}
// usleep(100000);
// printf("magxyz %d %d %d ", mrb.x_raw, mrb.y_raw, mrb.z_raw);
printf("magxyz %8.4f %8.4f %8.4f ", (double)mrb.x, (double)mrb.y, (double)mrb.z);
/* Sleep 100 ms waiting for user input five times ~ 1s */
for (int k = 0; k < (11 - rate); k++) {
struct pollfd fds;
int ret;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
ret = poll(&fds, 1, 0);
if (ret > 0) {
char c;
uint8_t cnt0 = (mrb.error_count & 0x000000FF00000000) >> 32;
uint8_t cnt1 = (mrb.error_count & 0x00000000FF000000) >> 24;
uint8_t cnt2 = (mrb.error_count & 0x0000000000FF0000) >> 16;
uint8_t cnt3 = (mrb.error_count & 0x000000000000FF00) >> 8;
uint8_t cnt4 = (mrb.error_count & 0x00000000000000FF);
printf("[%x %x %x %x %x] ", cnt0, cnt1, cnt2, cnt3, cnt4);
read(0, &c, 1);
switch (c) {
case 0x03: // ctrl-c
case 0x1b: // esc
case 'c':
case 'q':
return OK;
/* not reached */
}
uint32_t mag_interval = (mrb.error_count & 0xFFFFFF0000000000) >> 40;
printf("%u ", mag_interval);
if ((mag_interval) < 5000) {
printf("* ");
}
usleep(100000);
if ((mag_interval) > 15000) {
printf("! ");
}
if (mrb.x_raw == x_raw && mrb.y_raw == y_raw && mrb.z_raw == z_raw ) {
printf("dup "); // duplicate magnetometer xyz data
}
x_raw = mrb.x_raw;
y_raw = mrb.y_raw;
z_raw = mrb.z_raw;
printf("\n");
}
struct pollfd fds;
int ret;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
ret = poll(&fds, 1, 0);
if (ret > 0) {
char c;
read(0, &c, 1);
switch (c) {
case 0x03: // ctrl-c
case 0x1b: // esc
case 'c':
case 'q':
return OK;
}
}
}
return OK;
}
#define MAX_SAMPLES 100
static struct mag_report mrb[MAX_SAMPLES];
static uint64_t last_timestamp;
int
mag2(void)
{
int i = 0;
int mag_fd = orb_subscribe(ORB_ID(sensor_mag));
while (i < MAX_SAMPLES) {
bool updated = false;
orb_check(mag_fd, &updated);
if (updated) {
orb_copy(ORB_ID(sensor_mag), mag_fd, &mrb[i++]);
}
/*
struct pollfd fds;
int ret;
fds.fd = 0; // stdin
fds.events = POLLIN;
ret = poll(&fds, 1, 0);
if (ret > 0) {
char c;
read(0, &c, 1);
switch (c) {
case 0x03: // ctrl-c
case 0x1b: // esc
case 'c':
case 'q':
return OK;
}
}
*/
}
uint32_t total_time = 0;
last_timestamp = mrb[0].timestamp;
for (i = 0; i < MAX_SAMPLES; i++) {
// printf("magxyz %d %d %d ", mrb[i].x_raw, mrb[i].y_raw, mrb[i].z_raw);
printf("magxyz %8.4f %8.4f %8.4f ", (double)mrb[i].x, (double)mrb[i].y, (double)mrb[i].z);
uint8_t cnt0 = (mrb[i].error_count & 0x000000FF00000000) >> 32;
uint8_t cnt1 = (mrb[i].error_count & 0x00000000FF000000) >> 24;
uint8_t cnt2 = (mrb[i].error_count & 0x0000000000FF0000) >> 16;
uint8_t cnt3 = (mrb[i].error_count & 0x000000000000FF00) >> 8;
uint8_t cnt4 = (mrb[i].error_count & 0x00000000000000FF);
printf("[%x %x %x %x %x] ", cnt0, cnt1, cnt2, cnt3, cnt4);
uint32_t mag_interval = (mrb[i].error_count & 0xFFFFFF0000000000) >> 40;
printf("%u ", mag_interval);
if ((mag_interval) < 5000) {
printf("* ");
}
if ((mag_interval) > 15000) {
printf("! ");
}
uint32_t orb_interval = mrb[i].timestamp - last_timestamp;
if (orb_interval != mag_interval) {
printf("orb ", orb_interval); // missed some orb publications
}
last_timestamp = mrb[i].timestamp;
total_time += orb_interval;
if (mrb[i].x_raw == mrb[i-1].x_raw && mrb[i].y_raw == mrb[i-1].y_raw && mrb[i].z_raw == mrb[i-1].z_raw ) {
printf("dup "); // duplicate magnetometer xyz data
}
printf("\n");
}
double t = (double)total_time / 1000000.0;
printf("total_time: %.3f s\n", t);
return OK;
}
__EXPORT int mag_main(int argc, char *argv[]);
int
mag_main(int argc, char *argv[])
{
if (argc < 2) {
mag1();
} else {
int reps = atoi(argv[1]);
if (reps > 20 || reps < 1) {
reps = 1;
}
while (reps--) {
mag2();
}
}
return OK;
}
src/systemcmds/mag/module.mk
+1 -1
View File
@@ -32,7 +32,7 @@
############################################################################
#
# Build nshterm utility
# Build mag utility
#
MODULE_COMMAND = mag