/**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /** * @file mpu6000.cpp * * Driver for the Invensense MPU6000 connected via SPI. */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #define DIR_READ 0x80 #define DIR_WRITE 0x00 // MPU 6000 registers #define MPUREG_WHOAMI 0x75 #define MPUREG_SMPLRT_DIV 0x19 #define MPUREG_CONFIG 0x1A #define MPUREG_GYRO_CONFIG 0x1B #define MPUREG_ACCEL_CONFIG 0x1C #define MPUREG_FIFO_EN 0x23 #define MPUREG_INT_PIN_CFG 0x37 #define MPUREG_INT_ENABLE 0x38 #define MPUREG_INT_STATUS 0x3A #define MPUREG_ACCEL_XOUT_H 0x3B #define MPUREG_ACCEL_XOUT_L 0x3C #define MPUREG_ACCEL_YOUT_H 0x3D #define MPUREG_ACCEL_YOUT_L 0x3E #define MPUREG_ACCEL_ZOUT_H 0x3F #define MPUREG_ACCEL_ZOUT_L 0x40 #define MPUREG_TEMP_OUT_H 0x41 #define MPUREG_TEMP_OUT_L 0x42 #define MPUREG_GYRO_XOUT_H 0x43 #define MPUREG_GYRO_XOUT_L 0x44 #define MPUREG_GYRO_YOUT_H 0x45 #define MPUREG_GYRO_YOUT_L 0x46 #define MPUREG_GYRO_ZOUT_H 0x47 #define MPUREG_GYRO_ZOUT_L 0x48 #define MPUREG_USER_CTRL 0x6A #define MPUREG_PWR_MGMT_1 0x6B #define MPUREG_PWR_MGMT_2 0x6C #define MPUREG_FIFO_COUNTH 0x72 #define MPUREG_FIFO_COUNTL 0x73 #define MPUREG_FIFO_R_W 0x74 #define MPUREG_PRODUCT_ID 0x0C // Configuration bits MPU 3000 and MPU 6000 (not revised)? #define BIT_SLEEP 0x40 #define BIT_H_RESET 0x80 #define BITS_CLKSEL 0x07 #define MPU_CLK_SEL_PLLGYROX 0x01 #define MPU_CLK_SEL_PLLGYROZ 0x03 #define MPU_EXT_SYNC_GYROX 0x02 #define BITS_FS_250DPS 0x00 #define BITS_FS_500DPS 0x08 #define BITS_FS_1000DPS 0x10 #define BITS_FS_2000DPS 0x18 #define BITS_FS_MASK 0x18 #define BITS_DLPF_CFG_256HZ_NOLPF2 0x00 #define BITS_DLPF_CFG_188HZ 0x01 #define BITS_DLPF_CFG_98HZ 0x02 #define BITS_DLPF_CFG_42HZ 0x03 #define BITS_DLPF_CFG_20HZ 0x04 #define BITS_DLPF_CFG_10HZ 0x05 #define BITS_DLPF_CFG_5HZ 0x06 #define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 #define BITS_DLPF_CFG_MASK 0x07 #define BIT_INT_ANYRD_2CLEAR 0x10 #define BIT_RAW_RDY_EN 0x01 #define BIT_I2C_IF_DIS 0x10 #define BIT_INT_STATUS_DATA 0x01 // Product ID Description for MPU6000 // high 4 bits low 4 bits // Product Name Product Revision #define MPU6000ES_REV_C4 0x14 #define MPU6000ES_REV_C5 0x15 #define MPU6000ES_REV_D6 0x16 #define MPU6000ES_REV_D7 0x17 #define MPU6000ES_REV_D8 0x18 #define MPU6000_REV_C4 0x54 #define MPU6000_REV_C5 0x55 #define MPU6000_REV_D6 0x56 #define MPU6000_REV_D7 0x57 #define MPU6000_REV_D8 0x58 #define MPU6000_REV_D9 0x59 #define MPU6000_REV_D10 0x5A class MPU6000_gyro; class MPU6000 : public device::SPI { public: MPU6000(int bus, spi_dev_e device); virtual ~MPU6000(); virtual int init(); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. */ void print_info(); protected: virtual int probe(); friend class MPU6000_gyro; virtual ssize_t gyro_read(struct file *filp, char *buffer, size_t buflen); virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg); private: MPU6000_gyro *_gyro; uint8_t _product; /** product code */ struct hrt_call _call; unsigned _call_interval; struct accel_report _accel_report; struct accel_scale _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; struct gyro_report _gyro_report; struct gyro_scale _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; orb_advert_t _gyro_topic; unsigned _reads; perf_counter_t _sample_perf; /** * Start automatic measurement. */ void start(); /** * Stop automatic measurement. */ void stop(); /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. * * Called by the HRT in interrupt context at the specified rate if * automatic polling is enabled. * * @param arg Instance pointer for the driver that is polling. */ static void measure_trampoline(void *arg); /** * Fetch measurements from the sensor and update the report ring. */ void measure(); /** * Read a register from the MPU6000 * * @param The register to read. * @return The value that was read. */ uint8_t read_reg(unsigned reg); uint16_t read_reg16(unsigned reg); /** * Write a register in the MPU6000 * * @param reg The register to write. * @param value The new value to write. */ void write_reg(unsigned reg, uint8_t value); /** * Modify a register in the MPU6000 * * Bits are cleared before bits are set. * * @param reg The register to modify. * @param clearbits Bits in the register to clear. * @param setbits Bits in the register to set. */ void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); /** * Set the MPU6000 measurement range. * * @param max_g The maximum G value the range must support. * @return OK if the value can be supported, -ERANGE otherwise. */ int set_range(unsigned max_g); /** * Swap a 16-bit value read from the MPU6000 to native byte order. */ uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } /** * Self test * * @return 0 on success, 1 on failure */ int self_test(); /* set low pass filter frequency */ void _set_dlpf_filter(uint16_t frequency_hz); /* set sample rate (approximate) - 1kHz to 5Hz */ void _set_sample_rate(uint16_t desired_sample_rate_hz); }; /** * Helper class implementing the gyro driver node. */ class MPU6000_gyro : public device::CDev { public: MPU6000_gyro(MPU6000 *parent); ~MPU6000_gyro(); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); protected: friend class MPU6000; void parent_poll_notify(); private: MPU6000 *_parent; }; /** driver 'main' command */ extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } MPU6000::MPU6000(int bus, spi_dev_e device) : SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000), _gyro(new MPU6000_gyro(this)), _product(0), _call_interval(0), _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), _reads(0), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")) { // disable debug() calls _debug_enabled = false; // default accel scale factors _accel_scale.x_offset = 0; _accel_scale.x_scale = 1.0f; _accel_scale.y_offset = 0; _accel_scale.y_scale = 1.0f; _accel_scale.z_offset = 0; _accel_scale.z_scale = 1.0f; // default gyro scale factors _gyro_scale.x_offset = 0; _gyro_scale.x_scale = 1.0f; _gyro_scale.y_offset = 0; _gyro_scale.y_scale = 1.0f; _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; memset(&_accel_report, 0, sizeof(_accel_report)); memset(&_gyro_report, 0, sizeof(_gyro_report)); memset(&_call, 0, sizeof(_call)); } MPU6000::~MPU6000() { /* make sure we are truly inactive */ stop(); /* delete the gyro subdriver */ delete _gyro; /* delete the perf counter */ perf_free(_sample_perf); } int MPU6000::init() { int ret; /* do SPI init (and probe) first */ ret = SPI::init(); /* if probe/setup failed, bail now */ if (ret != OK) { debug("SPI setup failed"); return ret; } /* advertise sensor topics */ _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_report); _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_report); // Chip reset write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); up_udelay(10000); // Wake up device and select GyroZ clock (better performance) write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); up_udelay(1000); // Disable I2C bus (recommended on datasheet) write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); up_udelay(1000); // SAMPLE RATE //write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz _set_sample_rate(200); // default sample rate = 200Hz usleep(1000); // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) // was 90 Hz, but this ruins quality and does not improve the // system response _set_dlpf_filter(20); usleep(1000); // Gyro scale 2000 deg/s () write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); usleep(1000); // correct gyro scale factors // scale to rad/s in SI units // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s // scaling factor: // 1/(2^15)*(2000/180)*PI _gyro_scale.x_offset = 0; _gyro_scale.x_scale = 1.0f; _gyro_scale.y_offset = 0; _gyro_scale.y_scale = 1.0f; _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; // product-specific scaling switch (_product) { case MPU6000ES_REV_C4: case MPU6000ES_REV_C5: case MPU6000_REV_C4: case MPU6000_REV_C5: // Accel scale 8g (4096 LSB/g) // Rev C has different scaling than rev D write_reg(MPUREG_ACCEL_CONFIG, 1 << 3); break; case MPU6000ES_REV_D6: case MPU6000ES_REV_D7: case MPU6000ES_REV_D8: case MPU6000_REV_D6: case MPU6000_REV_D7: case MPU6000_REV_D8: case MPU6000_REV_D9: case MPU6000_REV_D10: // default case to cope with new chip revisions, which // presumably won't have the accel scaling bug default: // Accel scale 8g (4096 LSB/g) write_reg(MPUREG_ACCEL_CONFIG, 2 << 3); break; } // Correct accel scale factors of 4096 LSB/g // scale to m/s^2 ( 1g = 9.81 m/s^2) _accel_scale.x_offset = 0; _accel_scale.x_scale = 1.0f; _accel_scale.y_offset = 0; _accel_scale.y_scale = 1.0f; _accel_scale.z_offset = 0; _accel_scale.z_scale = 1.0f; _accel_range_scale = (9.81f / 4096.0f); _accel_range_m_s2 = 8.0f * 9.81f; usleep(1000); // INT CFG => Interrupt on Data Ready write_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready usleep(1000); write_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read usleep(1000); // Oscillator set // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); usleep(1000); /* do CDev init for the gyro device node, keep it optional */ int gyro_ret = _gyro->init(); if (gyro_ret != OK) { _gyro_topic = -1; } return ret; } int MPU6000::probe() { /* look for a product ID we recognise */ _product = read_reg(MPUREG_PRODUCT_ID); // verify product revision switch (_product) { case MPU6000ES_REV_C4: case MPU6000ES_REV_C5: case MPU6000_REV_C4: case MPU6000_REV_C5: case MPU6000ES_REV_D6: case MPU6000ES_REV_D7: case MPU6000ES_REV_D8: case MPU6000_REV_D6: case MPU6000_REV_D7: case MPU6000_REV_D8: case MPU6000_REV_D9: case MPU6000_REV_D10: debug("ID 0x%02x", _product); return OK; } debug("unexpected ID 0x%02x", _product); return -EIO; } /* set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro */ void MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz) { uint8_t div = 1000 / desired_sample_rate_hz; if(div>200) div=200; if(div<1) div=1; write_reg(MPUREG_SMPLRT_DIV, div-1); } /* set the DLPF filter frequency. This affects both accel and gyro. */ void MPU6000::_set_dlpf_filter(uint16_t frequency_hz) { uint8_t filter; /* choose next highest filter frequency available */ if (frequency_hz <= 5) { filter = BITS_DLPF_CFG_5HZ; } else if (frequency_hz <= 10) { filter = BITS_DLPF_CFG_10HZ; } else if (frequency_hz <= 20) { filter = BITS_DLPF_CFG_20HZ; } else if (frequency_hz <= 42) { filter = BITS_DLPF_CFG_42HZ; } else if (frequency_hz <= 98) { filter = BITS_DLPF_CFG_98HZ; } else if (frequency_hz <= 188) { filter = BITS_DLPF_CFG_188HZ; } else if (frequency_hz <= 256) { filter = BITS_DLPF_CFG_256HZ_NOLPF2; } else { filter = BITS_DLPF_CFG_2100HZ_NOLPF; } write_reg(MPUREG_CONFIG, filter); } ssize_t MPU6000::read(struct file *filp, char *buffer, size_t buflen) { int ret = 0; /* buffer must be large enough */ if (buflen < sizeof(_accel_report)) return -ENOSPC; /* if automatic measurement is not enabled */ if (_call_interval == 0) measure(); /* copy out the latest reports */ memcpy(buffer, &_accel_report, sizeof(_accel_report)); ret = sizeof(_accel_report); return ret; } int MPU6000::self_test() { if (_reads == 0) { measure(); } /* return 0 on success, 1 else */ return (_reads > 0) ? 0 : 1; } ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { int ret = 0; /* buffer must be large enough */ if (buflen < sizeof(_gyro_report)) return -ENOSPC; /* if automatic measurement is not enabled */ if (_call_interval == 0) measure(); /* copy out the latest report */ memcpy(buffer, &_gyro_report, sizeof(_gyro_report)); ret = sizeof(_gyro_report); return ret; } int MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case SENSORIOCSPOLLRATE: { switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: /* zero would be bad */ case 0: return -EINVAL; /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: /* XXX 500Hz is just a wild guess */ return ioctl(filp, SENSORIOCSPOLLRATE, 500); /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_call_interval == 0); /* convert hz to hrt interval via microseconds */ unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ if (ticks < 1000) return -EINVAL; /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _call.period = _call_interval = ticks; /* if we need to start the poll state machine, do it */ if (want_start) start(); return OK; } } } case SENSORIOCGPOLLRATE: if (_call_interval == 0) return SENSOR_POLLRATE_MANUAL; return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: /* XXX not implemented */ return -EINVAL; case SENSORIOCGQUEUEDEPTH: /* XXX not implemented */ return -EINVAL; case ACCELIOCSSAMPLERATE: case ACCELIOCGSAMPLERATE: _set_sample_rate(arg); return OK; case ACCELIOCSLOWPASS: case ACCELIOCGLOWPASS: _set_dlpf_filter((uint16_t)arg); return OK; case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ struct accel_scale *s = (struct accel_scale *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; if (sum > 2.0f && sum < 4.0f) { memcpy(&_accel_scale, s, sizeof(_accel_scale)); return OK; } else { return -EINVAL; } } case ACCELIOCGSCALE: /* copy scale out */ memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: case ACCELIOCGRANGE: /* XXX not implemented */ // XXX change these two values on set: // _accel_range_scale = (9.81f / 4096.0f); // _accel_range_rad_s = 8.0f * 9.81f; return -EINVAL; case ACCELIOCSELFTEST: return self_test(); default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); } } int MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCSQUEUEDEPTH: case SENSORIOCGQUEUEDEPTH: case SENSORIOCRESET: return ioctl(filp, cmd, arg); case GYROIOCSSAMPLERATE: case GYROIOCGSAMPLERATE: _set_sample_rate(arg); return OK; case GYROIOCSLOWPASS: case GYROIOCGLOWPASS: _set_dlpf_filter((uint16_t)arg); return OK; case GYROIOCSSCALE: /* copy scale in */ memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: case GYROIOCGRANGE: /* XXX not implemented */ // XXX change these two values on set: // _gyro_range_scale = xx // _gyro_range_m_s2 = xx return -EINVAL; case GYROIOCSELFTEST: return self_test(); default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); } } uint8_t MPU6000::read_reg(unsigned reg) { uint8_t cmd[2]; cmd[0] = reg | DIR_READ; transfer(cmd, cmd, sizeof(cmd)); return cmd[1]; } uint16_t MPU6000::read_reg16(unsigned reg) { uint8_t cmd[3]; cmd[0] = reg | DIR_READ; transfer(cmd, cmd, sizeof(cmd)); return (uint16_t)(cmd[1] << 8) | cmd[2]; } void MPU6000::write_reg(unsigned reg, uint8_t value) { uint8_t cmd[2]; cmd[0] = reg | DIR_WRITE; cmd[1] = value; transfer(cmd, nullptr, sizeof(cmd)); } void MPU6000::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) { uint8_t val; val = read_reg(reg); val &= ~clearbits; val |= setbits; write_reg(reg, val); } int MPU6000::set_range(unsigned max_g) { #if 0 uint8_t rangebits; float rangescale; if (max_g > 16) { return -ERANGE; } else if (max_g > 8) { /* 16G */ rangebits = OFFSET_LSB1_RANGE_16G; rangescale = 1.98; } else if (max_g > 4) { /* 8G */ rangebits = OFFSET_LSB1_RANGE_8G; rangescale = 0.99; } else if (max_g > 3) { /* 4G */ rangebits = OFFSET_LSB1_RANGE_4G; rangescale = 0.5; } else if (max_g > 2) { /* 3G */ rangebits = OFFSET_LSB1_RANGE_3G; rangescale = 0.38; } else if (max_g > 1) { /* 2G */ rangebits = OFFSET_LSB1_RANGE_2G; rangescale = 0.25; } else { /* 1G */ rangebits = OFFSET_LSB1_RANGE_1G; rangescale = 0.13; } /* adjust sensor configuration */ modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits); _range_scale = rangescale; #endif return OK; } void MPU6000::start() { /* make sure we are stopped first */ stop(); /* start polling at the specified rate */ hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this); } void MPU6000::stop() { hrt_cancel(&_call); } void MPU6000::measure_trampoline(void *arg) { MPU6000 *dev = (MPU6000 *)arg; /* make another measurement */ dev->measure(); } void MPU6000::measure() { #pragma pack(push, 1) /** * Report conversation within the MPU6000, including command byte and * interrupt status. */ struct MPUReport { uint8_t cmd; uint8_t status; uint8_t accel_x[2]; uint8_t accel_y[2]; uint8_t accel_z[2]; uint8_t temp[2]; uint8_t gyro_x[2]; uint8_t gyro_y[2]; uint8_t gyro_z[2]; } mpu_report; #pragma pack(pop) struct Report { int16_t accel_x; int16_t accel_y; int16_t accel_z; int16_t temp; int16_t gyro_x; int16_t gyro_y; int16_t gyro_z; } report; /* start measuring */ perf_begin(_sample_perf); /* * Fetch the full set of measurements from the MPU6000 in one pass. */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) return; /* count measurement */ _reads++; /* * 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); /* * Swap axes and negate y */ int16_t accel_xt = report.accel_y; int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); int16_t gyro_xt = report.gyro_y; int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); /* * Apply the swap */ report.accel_x = accel_xt; report.accel_y = accel_yt; report.gyro_x = gyro_xt; report.gyro_y = gyro_yt; /* * Adjust and scale results to m/s^2. */ _gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time(); /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) * 3) Scale the statically calibrated values with a linear * dynamically obtained factor * * Note: the static sensor offset is the number the sensor outputs * at a nominally 'zero' input. Therefore the offset has to * be subtracted. * * Example: A gyro outputs a value of 74 at zero angular rate * the offset is 74 from the origin and subtracting * 74 from all measurements centers them around zero. */ /* NOTE: Axes have been swapped to match the board a few lines above. */ _accel_report.x_raw = report.accel_x; _accel_report.y_raw = report.accel_y; _accel_report.z_raw = report.accel_z; _accel_report.x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; _accel_report.y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; _accel_report.z = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; _accel_report.scaling = _accel_range_scale; _accel_report.range_m_s2 = _accel_range_m_s2; _accel_report.temperature_raw = report.temp; _accel_report.temperature = (report.temp) / 361.0f + 35.0f; _gyro_report.x_raw = report.gyro_x; _gyro_report.y_raw = report.gyro_y; _gyro_report.z_raw = report.gyro_z; _gyro_report.x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; _gyro_report.y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; _gyro_report.z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; _gyro_report.scaling = _gyro_range_scale; _gyro_report.range_rad_s = _gyro_range_rad_s; _gyro_report.temperature_raw = report.temp; _gyro_report.temperature = (report.temp) / 361.0f + 35.0f; /* notify anyone waiting for data */ poll_notify(POLLIN); _gyro->parent_poll_notify(); /* and publish for subscribers */ orb_publish(ORB_ID(sensor_accel), _accel_topic, &_accel_report); if (_gyro_topic != -1) { orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report); } /* stop measuring */ perf_end(_sample_perf); } void MPU6000::print_info() { printf("reads: %u\n", _reads); } MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : CDev("MPU6000_gyro", GYRO_DEVICE_PATH), _parent(parent) { } MPU6000_gyro::~MPU6000_gyro() { } void MPU6000_gyro::parent_poll_notify() { poll_notify(POLLIN); } ssize_t MPU6000_gyro::read(struct file *filp, char *buffer, size_t buflen) { return _parent->gyro_read(filp, buffer, buflen); } int MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) { return _parent->gyro_ioctl(filp, cmd, arg); } /** * Local functions in support of the shell command. */ namespace mpu6000 { MPU6000 *g_dev; void start(); void test(); void reset(); void info(); /** * Start the driver. */ void start() { int fd; if (g_dev != nullptr) errx(1, "already started"); /* create the driver */ g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU); if (g_dev == nullptr) goto fail; if (OK != g_dev->init()) goto fail; /* set the poll rate to default, starts automatic data collection */ fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) goto fail; if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; exit(0); fail: if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } errx(1, "driver start failed"); } /** * Perform some basic functional tests on the driver; * make sure we can collect data from the sensor in polled * and automatic modes. */ void test() { int fd = -1; int fd_gyro = -1; struct accel_report a_report; struct gyro_report g_report; ssize_t sz; /* get the driver */ fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", ACCEL_DEVICE_PATH); /* get the driver */ fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); if (fd_gyro < 0) err(1, "%s open failed", GYRO_DEVICE_PATH); /* reset to manual polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) err(1, "reset to manual polling"); /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); if (sz != sizeof(a_report)) err(1, "immediate acc read failed"); warnx("single read"); warnx("time: %lld", a_report.timestamp); warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x); warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y); warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z); warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw); warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, (double)(a_report.range_m_s2 / 9.81f)); /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); if (sz != sizeof(g_report)) err(1, "immediate gyro read failed"); warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z); warnx("gyro x: \t%d\traw", (int)g_report.x_raw); warnx("gyro y: \t%d\traw", (int)g_report.y_raw); warnx("gyro z: \t%d\traw", (int)g_report.z_raw); warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); /* XXX add poll-rate tests here too */ reset(); errx(0, "PASS"); } /** * Reset the driver. */ void reset() { int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "failed "); if (ioctl(fd, SENSORIOCRESET, 0) < 0) err(1, "driver reset failed"); if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); exit(0); } /** * Print a little info about the driver. */ void info() { if (g_dev == nullptr) errx(1, "driver not running"); printf("state @ %p\n", g_dev); g_dev->print_info(); exit(0); } } // namespace int mpu6000_main(int argc, char *argv[]) { /* * Start/load the driver. */ if (!strcmp(argv[1], "start")) mpu6000::start(); /* * Test the driver/device. */ if (!strcmp(argv[1], "test")) mpu6000::test(); /* * Reset the driver. */ if (!strcmp(argv[1], "reset")) mpu6000::reset(); /* * Print driver information. */ if (!strcmp(argv[1], "info")) mpu6000::info(); errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); }