diff options
Diffstat (limited to 'apps/drivers/mpu6000/mpu6000.cpp')
-rw-r--r-- | apps/drivers/mpu6000/mpu6000.cpp | 1219 |
1 files changed, 0 insertions, 1219 deletions
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp deleted file mode 100644 index ce7062046..000000000 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ /dev/null @@ -1,1219 +0,0 @@ -/**************************************************************************** - * - * 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 <nuttx/config.h> - -#include <sys/types.h> -#include <stdint.h> -#include <stdbool.h> -#include <stddef.h> -#include <stdlib.h> -#include <semaphore.h> -#include <string.h> -#include <fcntl.h> -#include <poll.h> -#include <errno.h> -#include <stdio.h> -#include <math.h> -#include <unistd.h> - -#include <systemlib/perf_counter.h> -#include <systemlib/err.h> -#include <systemlib/conversions.h> - -#include <nuttx/arch.h> -#include <nuttx/clock.h> - -#include <arch/board/board.h> -#include <drivers/drv_hrt.h> - -#include <drivers/device/spi.h> -#include <drivers/drv_accel.h> -#include <drivers/drv_gyro.h> - -#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); - -}; - -/** - * 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 - 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 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: - /* XXX not implemented */ - return -EINVAL; - - 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: - /* XXX not implemented */ - return -EINVAL; - - 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'"); -} |