From 5903891213251bbabbbc3de6a5e83ad18bb0a3e8 Mon Sep 17 00:00:00 2001 From: Robert Dickenson Date: Thu, 3 Mar 2016 16:28:08 +1100 Subject: [PATCH] Significant work on mpu9250 driver to get magnetometer updates happening reliably. Improved 'mag' utility to aid driver development and testing. --- src/drivers/mpu9250/CMakeLists.txt | 2 +- src/drivers/mpu9250/gyro.cpp | 1 + src/drivers/mpu9250/gyro.h | 12 +- src/drivers/mpu9250/mag.cpp | 672 ++++++++++++++++++++++------- src/drivers/mpu9250/mag.h | 54 ++- src/drivers/mpu9250/main.cpp | 137 +++--- src/drivers/mpu9250/mpu9250.cpp | 570 ++++-------------------- src/drivers/mpu9250/mpu9250.h | 141 ++---- src/systemcmds/mag/CMakeLists.txt | 2 +- src/systemcmds/mag/mag.c | 195 +++++++-- src/systemcmds/mag/module.mk | 2 +- 11 files changed, 896 insertions(+), 892 deletions(-) mode change 100644 => 100755 src/drivers/mpu9250/mag.cpp diff --git a/src/drivers/mpu9250/CMakeLists.txt b/src/drivers/mpu9250/CMakeLists.txt index d25a4127d6..bfbf3f7565 100755 --- a/src/drivers/mpu9250/CMakeLists.txt +++ b/src/drivers/mpu9250/CMakeLists.txt @@ -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 diff --git a/src/drivers/mpu9250/gyro.cpp b/src/drivers/mpu9250/gyro.cpp index fa609a1ead..ac86e32231 100644 --- a/src/drivers/mpu9250/gyro.cpp +++ b/src/drivers/mpu9250/gyro.cpp @@ -65,6 +65,7 @@ #include #include +#include "mag.h" #include "gyro.h" #include "mpu9250.h" diff --git a/src/drivers/mpu9250/gyro.h b/src/drivers/mpu9250/gyro.h index 67dcc2cd2f..b76b0f81bc 100644 --- a/src/drivers/mpu9250/gyro.h +++ b/src/drivers/mpu9250/gyro.h @@ -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 &); }; diff --git a/src/drivers/mpu9250/mag.cpp b/src/drivers/mpu9250/mag.cpp old mode 100644 new mode 100755 index f5bcd52261..3e19c8cfea --- a/src/drivers/mpu9250/mag.cpp +++ b/src/drivers/mpu9250/mag.cpp @@ -33,6 +33,11 @@ /** * @file mag.cpp + * + * Driver for the ak8963 magnetometer within the Invensense mpu9250. + * + * @author Robert Dickenson + * */ #include @@ -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(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 diff --git a/src/drivers/mpu9250/mag.h b/src/drivers/mpu9250/mag.h index 95224f6959..aa14964af3 100644 --- a/src/drivers/mpu9250/mag.h +++ b/src/drivers/mpu9250/mag.h @@ -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 &); }; - diff --git a/src/drivers/mpu9250/main.cpp b/src/drivers/mpu9250/main.cpp index 84af4cdab6..256635f416 100644 --- a/src/drivers/mpu9250/main.cpp +++ b/src/drivers/mpu9250/main.cpp @@ -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 #include +#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); } diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp index c7754230ea..836d44278f 100644 --- a/src/drivers/mpu9250/mpu9250.cpp +++ b/src/drivers/mpu9250/mpu9250.cpp @@ -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(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; -} diff --git a/src/drivers/mpu9250/mpu9250.h b/src/drivers/mpu9250/mpu9250.h index a9f71c330b..53c5e043a2 100644 --- a/src/drivers/mpu9250/mpu9250.h +++ b/src/drivers/mpu9250/mpu9250.h @@ -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) }; - diff --git a/src/systemcmds/mag/CMakeLists.txt b/src/systemcmds/mag/CMakeLists.txt index 199004e301..6a5baae4aa 100644 --- a/src/systemcmds/mag/CMakeLists.txt +++ b/src/systemcmds/mag/CMakeLists.txt @@ -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 diff --git a/src/systemcmds/mag/mag.c b/src/systemcmds/mag/mag.c index 07a1c1e297..0c9105ef93 100644 --- a/src/systemcmds/mag/mag.c +++ b/src/systemcmds/mag/mag.c @@ -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 @@ -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; } diff --git a/src/systemcmds/mag/module.mk b/src/systemcmds/mag/module.mk index 86ed458bb5..3072f7776c 100644 --- a/src/systemcmds/mag/module.mk +++ b/src/systemcmds/mag/module.mk @@ -32,7 +32,7 @@ ############################################################################ # -# Build nshterm utility +# Build mag utility # MODULE_COMMAND = mag