Skip to content

D
Drone Controller
Commit 097a5bd4 authored by 15김건우's avatar 15김건우
Browse files
Options

Final

parents
Pipeline #55 failed with stages
      in 44 seconds
      Hide whitespace changes
      Inline Side-by-side
      Showing with 1525 additions and 0 deletions
      MPU9250.cpp 0 → 100644
      View file @ 097a5bd4
      /*
      MPU9250.cpp
      Brian R Taylor
      brian.taylor@bolderflight.com
      Copyright (c) 2017 Bolder Flight Systems
      Permission is hereby granted, free of charge, to any person obtaining a copy of this software
      and associated documentation files (the "Software"), to deal in the Software without restriction,
      including without limitation the rights to use, copy, modify, merge, publish, distribute,
      sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
      furnished to do so, subject to the following conditions:
      The above copyright notice and this permission notice shall be included in all copies or
      substantial portions of the Software.
      THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
      BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
      NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
      DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
      OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
      */
      #include "Arduino.h"
      #include "MPU9250.h"
      /* MPU9250 object, input the I2C bus and address */
      MPU9250::MPU9250(TwoWire &bus,uint8_t address){
      _i2c = &bus; // I2C bus
      _address = address; // I2C address
      _useSPI = false; // set to use I2C
      }
      /* MPU9250 object, input the SPI bus and chip select pin */
      MPU9250::MPU9250(SPIClass &bus,uint8_t csPin){
      _spi = &bus; // SPI bus
      _csPin = csPin; // chip select pin
      _useSPI = true; // set to use SPI
      }
      /* starts communication with the MPU-9250 */
      int MPU9250::begin(){
      if( _useSPI ) { // using SPI for communication
      // use low speed SPI for register setting
      _useSPIHS = false;
      // setting CS pin to output
      pinMode(_csPin,OUTPUT);
      // setting CS pin high
      digitalWrite(_csPin,HIGH);
      // begin SPI communication
      _spi->begin();
      } else { // using I2C for communication
      // starting the I2C bus
      _i2c->begin();
      // setting the I2C clock
      _i2c->setClock(_i2cRate);
      }
      // select clock source to gyro
      if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0){
      return -1;
      }
      // enable I2C master mode
      if(writeRegister(USER_CTRL,I2C_MST_EN) < 0){
      return -2;
      }
      // set the I2C bus speed to 400 kHz
      if(writeRegister(I2C_MST_CTRL,I2C_MST_CLK) < 0){
      return -3;
      }
      // set AK8963 to Power Down
      writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN);
      // reset the MPU9250
      writeRegister(PWR_MGMNT_1,PWR_RESET);
      // wait for MPU-9250 to come back up
      delay(1);
      // reset the AK8963
      writeAK8963Register(AK8963_CNTL2,AK8963_RESET);
      // select clock source to gyro
      if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0){
      return -4;
      }
      // check the WHO AM I byte, expected value is 0x71 (decimal 113) or 0x73 (decimal 115)
      if((whoAmI() != 113)&&(whoAmI() != 115)){
      return -5;
      }
      // enable accelerometer and gyro
      if(writeRegister(PWR_MGMNT_2,SEN_ENABLE) < 0){
      return -6;
      }
      // setting accel range to 16G as default
      if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_16G) < 0){
      return -7;
      }
      _accelScale = G * 16.0f/32767.5f; // setting the accel scale to 16G
      _accelRange = ACCEL_RANGE_16G;
      // setting the gyro range to 2000DPS as default
      if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_2000DPS) < 0){
      return -8;
      }
      _gyroScale = 2000.0f/32767.5f * _d2r; // setting the gyro scale to 2000DPS
      _gyroRange = GYRO_RANGE_2000DPS;
      // setting bandwidth to 184Hz as default
      if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) < 0){
      return -9;
      }
      if(writeRegister(CONFIG,GYRO_DLPF_184) < 0){ // setting gyro bandwidth to 184Hz
      return -10;
      }
      _bandwidth = DLPF_BANDWIDTH_184HZ;
      // setting the sample rate divider to 0 as default
      if(writeRegister(SMPDIV,0x00) < 0){
      return -11;
      }
      _srd = 0;
      // enable I2C master mode
      if(writeRegister(USER_CTRL,I2C_MST_EN) < 0){
      return -12;
      }
      // set the I2C bus speed to 400 kHz
      if( writeRegister(I2C_MST_CTRL,I2C_MST_CLK) < 0){
      return -13;
      }
      // check AK8963 WHO AM I register, expected value is 0x48 (decimal 72)
      if( whoAmIAK8963() != 72 ){
      return -14;
      }
      /* get the magnetometer calibration */
      // set AK8963 to Power Down
      if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){
      return -15;
      }
      delay(100); // long wait between AK8963 mode changes
      // set AK8963 to FUSE ROM access
      if(writeAK8963Register(AK8963_CNTL1,AK8963_FUSE_ROM) < 0){
      return -16;
      }
      delay(100); // long wait between AK8963 mode changes
      // read the AK8963 ASA registers and compute magnetometer scale factors
      readAK8963Registers(AK8963_ASA,3,_buffer);
      _magScaleX = ((((float)_buffer[0]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
      _magScaleY = ((((float)_buffer[1]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
      _magScaleZ = ((((float)_buffer[2]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
      // set AK8963 to Power Down
      if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){
      return -17;
      }
      delay(100); // long wait between AK8963 mode changes
      // set AK8963 to 16 bit resolution, 100 Hz update rate
      if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS2) < 0){
      return -18;
      }
      delay(100); // long wait between AK8963 mode changes
      // select clock source to gyro
      if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0){
      return -19;
      }
      // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate
      readAK8963Registers(AK8963_HXL,7,_buffer);
      // estimate gyro bias
      if (calibrateGyro() < 0) {
      return -20;
      }
      // successful init, return 1
      return 1;
      }
      /* sets the accelerometer full scale range to values other than default */
      int MPU9250::setAccelRange(AccelRange range) {
      // use low speed SPI for register setting
      _useSPIHS = false;
      switch(range) {
      case ACCEL_RANGE_2G: {
      // setting the accel range to 2G
      if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_2G) < 0){
      return -1;
      }
      _accelScale = G * 2.0f/32767.5f; // setting the accel scale to 2G
      break;
      }
      case ACCEL_RANGE_4G: {
      // setting the accel range to 4G
      if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_4G) < 0){
      return -1;
      }
      _accelScale = G * 4.0f/32767.5f; // setting the accel scale to 4G
      break;
      }
      case ACCEL_RANGE_8G: {
      // setting the accel range to 8G
      if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_8G) < 0){
      return -1;
      }
      _accelScale = G * 8.0f/32767.5f; // setting the accel scale to 8G
      break;
      }
      case ACCEL_RANGE_16G: {
      // setting the accel range to 16G
      if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_16G) < 0){
      return -1;
      }
      _accelScale = G * 16.0f/32767.5f; // setting the accel scale to 16G
      break;
      }
      }
      _accelRange = range;
      return 1;
      }
      /* sets the gyro full scale range to values other than default */
      int MPU9250::setGyroRange(GyroRange range) {
      // use low speed SPI for register setting
      _useSPIHS = false;
      switch(range) {
      case GYRO_RANGE_250DPS: {
      // setting the gyro range to 250DPS
      if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_250DPS) < 0){
      return -1;
      }
      _gyroScale = 250.0f/32767.5f * _d2r; // setting the gyro scale to 250DPS
      break;
      }
      case GYRO_RANGE_500DPS: {
      // setting the gyro range to 500DPS
      if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_500DPS) < 0){
      return -1;
      }
      _gyroScale = 500.0f/32767.5f * _d2r; // setting the gyro scale to 500DPS
      break;
      }
      case GYRO_RANGE_1000DPS: {
      // setting the gyro range to 1000DPS
      if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_1000DPS) < 0){
      return -1;
      }
      _gyroScale = 1000.0f/32767.5f * _d2r; // setting the gyro scale to 1000DPS
      break;
      }
      case GYRO_RANGE_2000DPS: {
      // setting the gyro range to 2000DPS
      if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_2000DPS) < 0){
      return -1;
      }
      _gyroScale = 2000.0f/32767.5f * _d2r; // setting the gyro scale to 2000DPS
      break;
      }
      }
      _gyroRange = range;
      return 1;
      }
      /* sets the DLPF bandwidth to values other than default */
      int MPU9250::setDlpfBandwidth(DlpfBandwidth bandwidth) {
      // use low speed SPI for register setting
      _useSPIHS = false;
      switch(bandwidth) {
      case DLPF_BANDWIDTH_184HZ: {
      if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) < 0){ // setting accel bandwidth to 184Hz
      return -1;
      }
      if(writeRegister(CONFIG,GYRO_DLPF_184) < 0){ // setting gyro bandwidth to 184Hz
      return -2;
      }
      break;
      }
      case DLPF_BANDWIDTH_92HZ: {
      if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_92) < 0){ // setting accel bandwidth to 92Hz
      return -1;
      }
      if(writeRegister(CONFIG,GYRO_DLPF_92) < 0){ // setting gyro bandwidth to 92Hz
      return -2;
      }
      break;
      }
      case DLPF_BANDWIDTH_41HZ: {
      if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_41) < 0){ // setting accel bandwidth to 41Hz
      return -1;
      }
      if(writeRegister(CONFIG,GYRO_DLPF_41) < 0){ // setting gyro bandwidth to 41Hz
      return -2;
      }
      break;
      }
      case DLPF_BANDWIDTH_20HZ: {
      if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_20) < 0){ // setting accel bandwidth to 20Hz
      return -1;
      }
      if(writeRegister(CONFIG,GYRO_DLPF_20) < 0){ // setting gyro bandwidth to 20Hz
      return -2;
      }
      break;
      }
      case DLPF_BANDWIDTH_10HZ: {
      if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_10) < 0){ // setting accel bandwidth to 10Hz
      return -1;
      }
      if(writeRegister(CONFIG,GYRO_DLPF_10) < 0){ // setting gyro bandwidth to 10Hz
      return -2;
      }
      break;
      }
      case DLPF_BANDWIDTH_5HZ: {
      if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_5) < 0){ // setting accel bandwidth to 5Hz
      return -1;
      }
      if(writeRegister(CONFIG,GYRO_DLPF_5) < 0){ // setting gyro bandwidth to 5Hz
      return -2;
      }
      break;
      }
      }
      _bandwidth = bandwidth;
      return 1;
      }
      /* sets the sample rate divider to values other than default */
      int MPU9250::setSrd(uint8_t srd) {
      // use low speed SPI for register setting
      _useSPIHS = false;
      /* setting the sample rate divider to 19 to facilitate setting up magnetometer */
      if(writeRegister(SMPDIV,19) < 0){ // setting the sample rate divider
      return -1;
      }
      if(srd > 9){
      // set AK8963 to Power Down
      if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){
      return -2;
      }
      delay(100); // long wait between AK8963 mode changes
      // set AK8963 to 16 bit resolution, 8 Hz update rate
      if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS1) < 0){
      return -3;
      }
      delay(100); // long wait between AK8963 mode changes
      // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate
      readAK8963Registers(AK8963_HXL,7,_buffer);
      } else {
      // set AK8963 to Power Down
      if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){
      return -2;
      }
      delay(100); // long wait between AK8963 mode changes
      // set AK8963 to 16 bit resolution, 100 Hz update rate
      if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS2) < 0){
      return -3;
      }
      delay(100); // long wait between AK8963 mode changes
      // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate
      readAK8963Registers(AK8963_HXL,7,_buffer);
      }
      /* setting the sample rate divider */
      if(writeRegister(SMPDIV,srd) < 0){ // setting the sample rate divider
      return -4;
      }
      _srd = srd;
      return 1;
      }
      /* enables the data ready interrupt */
      int MPU9250::enableDataReadyInterrupt() {
      // use low speed SPI for register setting
      _useSPIHS = false;
      /* setting the interrupt */
      if (writeRegister(INT_PIN_CFG,INT_PULSE_50US) < 0){ // setup interrupt, 50 us pulse
      return -1;
      }
      if (writeRegister(INT_ENABLE,INT_RAW_RDY_EN) < 0){ // set to data ready
      return -2;
      }
      return 1;
      }
      /* disables the data ready interrupt */
      int MPU9250::disableDataReadyInterrupt() {
      // use low speed SPI for register setting
      _useSPIHS = false;
      if(writeRegister(INT_ENABLE,INT_DISABLE) < 0){ // disable interrupt
      return -1;
      }
      return 1;
      }
      /* configures and enables wake on motion, low power mode */
      int MPU9250::enableWakeOnMotion(float womThresh_mg,LpAccelOdr odr) {
      // use low speed SPI for register setting
      _useSPIHS = false;
      // set AK8963 to Power Down
      writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN);
      // reset the MPU9250
      writeRegister(PWR_MGMNT_1,PWR_RESET);
      // wait for MPU-9250 to come back up
      delay(1);
      if(writeRegister(PWR_MGMNT_1,0x00) < 0){ // cycle 0, sleep 0, standby 0
      return -1;
      }
      if(writeRegister(PWR_MGMNT_2,DIS_GYRO) < 0){ // disable gyro measurements
      return -2;
      }
      if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) < 0){ // setting accel bandwidth to 184Hz
      return -3;
      }
      if(writeRegister(INT_ENABLE,INT_WOM_EN) < 0){ // enabling interrupt to wake on motion
      return -4;
      }
      if(writeRegister(MOT_DETECT_CTRL,(ACCEL_INTEL_EN | ACCEL_INTEL_MODE)) < 0){ // enabling accel hardware intelligence
      return -5;
      }
      _womThreshold = map(womThresh_mg, 0, 1020, 0, 255);
      if(writeRegister(WOM_THR,_womThreshold) < 0){ // setting wake on motion threshold
      return -6;
      }
      if(writeRegister(LP_ACCEL_ODR,(uint8_t)odr) < 0){ // set frequency of wakeup
      return -7;
      }
      if(writeRegister(PWR_MGMNT_1,PWR_CYCLE) < 0){ // switch to accel low power mode
      return -8;
      }
      return 1;
      }
      /* configures and enables the FIFO buffer */
      int MPU9250FIFO::enableFifo(bool accel,bool gyro,bool mag,bool temp) {
      // use low speed SPI for register setting
      _useSPIHS = false;
      if(writeRegister(USER_CTRL, (0x40 | I2C_MST_EN)) < 0){
      return -1;
      }
      if(writeRegister(FIFO_EN,(accel*FIFO_ACCEL)|(gyro*FIFO_GYRO)|(mag*FIFO_MAG)|(temp*FIFO_TEMP)) < 0){
      return -2;
      }
      _enFifoAccel = accel;
      _enFifoGyro = gyro;
      _enFifoMag = mag;
      _enFifoTemp = temp;
      _fifoFrameSize = accel*6 + gyro*6 + mag*7 + temp*2;
      return 1;
      }
      /* reads the most current data from MPU9250 and stores in buffer */
      int MPU9250::readSensor() {
      _useSPIHS = true; // use the high speed SPI for data readout
      // grab the data from the MPU9250
      if (readRegisters(ACCEL_OUT, 21, _buffer) < 0) {
      return -1;
      }
      // combine into 16 bit values
      _axcounts = (((int16_t)_buffer[0]) << 8) | _buffer[1];
      _aycounts = (((int16_t)_buffer[2]) << 8) | _buffer[3];
      _azcounts = (((int16_t)_buffer[4]) << 8) | _buffer[5];
      _tcounts = (((int16_t)_buffer[6]) << 8) | _buffer[7];
      _gxcounts = (((int16_t)_buffer[8]) << 8) | _buffer[9];
      _gycounts = (((int16_t)_buffer[10]) << 8) | _buffer[11];
      _gzcounts = (((int16_t)_buffer[12]) << 8) | _buffer[13];
      _hxcounts = (((int16_t)_buffer[15]) << 8) | _buffer[14];
      _hycounts = (((int16_t)_buffer[17]) << 8) | _buffer[16];
      _hzcounts = (((int16_t)_buffer[19]) << 8) | _buffer[18];
      // transform and convert to float values
      _ax = (((float)(tX[0]*_axcounts + tX[1]*_aycounts + tX[2]*_azcounts) * _accelScale) - _axb)*_axs;
      _ay = (((float)(tY[0]*_axcounts + tY[1]*_aycounts + tY[2]*_azcounts) * _accelScale) - _ayb)*_ays;
      _az = (((float)(tZ[0]*_axcounts + tZ[1]*_aycounts + tZ[2]*_azcounts) * _accelScale) - _azb)*_azs;
      _gx = ((float)(tX[0]*_gxcounts + tX[1]*_gycounts + tX[2]*_gzcounts) * _gyroScale) - _gxb;
      _gy = ((float)(tY[0]*_gxcounts + tY[1]*_gycounts + tY[2]*_gzcounts) * _gyroScale) - _gyb;
      _gz = ((float)(tZ[0]*_gxcounts + tZ[1]*_gycounts + tZ[2]*_gzcounts) * _gyroScale) - _gzb;
      _hx = (((float)(_hxcounts) * _magScaleX) - _hxb)*_hxs;
      _hy = (((float)(_hycounts) * _magScaleY) - _hyb)*_hys;
      _hz = (((float)(_hzcounts) * _magScaleZ) - _hzb)*_hzs;
      _t = ((((float) _tcounts) - _tempOffset)/_tempScale) + _tempOffset;
      return 1;
      }
      /* returns the accelerometer measurement in the x direction, m/s/s */
      float MPU9250::getAccelX_mss() {
      return _ax;
      }
      /* returns the accelerometer measurement in the y direction, m/s/s */
      float MPU9250::getAccelY_mss() {
      return _ay;
      }
      /* returns the accelerometer measurement in the z direction, m/s/s */
      float MPU9250::getAccelZ_mss() {
      return _az;
      }
      /* returns the gyroscope measurement in the x direction, rad/s */
      float MPU9250::getGyroX_rads() {
      return _gx;
      }
      /* returns the gyroscope measurement in the y direction, rad/s */
      float MPU9250::getGyroY_rads() {
      return _gy;
      }
      /* returns the gyroscope measurement in the z direction, rad/s */
      float MPU9250::getGyroZ_rads() {
      return _gz;
      }
      /* returns the magnetometer measurement in the x direction, uT */
      float MPU9250::getMagX_uT() {
      return _hx;
      }
      /* returns the magnetometer measurement in the y direction, uT */
      float MPU9250::getMagY_uT() {
      return _hy;
      }
      /* returns the magnetometer measurement in the z direction, uT */
      float MPU9250::getMagZ_uT() {
      return _hz;
      }
      /* returns the die temperature, C */
      float MPU9250::getTemperature_C() {
      return _t;
      }
      /* reads data from the MPU9250 FIFO and stores in buffer */
      int MPU9250FIFO::readFifo() {
      _useSPIHS = true; // use the high speed SPI for data readout
      // get the fifo size
      readRegisters(FIFO_COUNT, 2, _buffer);
      _fifoSize = (((uint16_t) (_buffer[0]&0x0F)) <<8) + (((uint16_t) _buffer[1]));
      // read and parse the buffer
      for (size_t i=0; i < _fifoSize/_fifoFrameSize; i++) {
      // grab the data from the MPU9250
      if (readRegisters(FIFO_READ,_fifoFrameSize,_buffer) < 0) {
      return -1;
      }
      if (_enFifoAccel) {
      // combine into 16 bit values
      _axcounts = (((int16_t)_buffer[0]) << 8) | _buffer[1];
      _aycounts = (((int16_t)_buffer[2]) << 8) | _buffer[3];
      _azcounts = (((int16_t)_buffer[4]) << 8) | _buffer[5];
      // transform and convert to float values
      _axFifo[i] = (((float)(tX[0]*_axcounts + tX[1]*_aycounts + tX[2]*_azcounts) * _accelScale)-_axb)*_axs;
      _ayFifo[i] = (((float)(tY[0]*_axcounts + tY[1]*_aycounts + tY[2]*_azcounts) * _accelScale)-_ayb)*_ays;
      _azFifo[i] = (((float)(tZ[0]*_axcounts + tZ[1]*_aycounts + tZ[2]*_azcounts) * _accelScale)-_azb)*_azs;
      _aSize = _fifoSize/_fifoFrameSize;
      }
      if (_enFifoTemp) {
      // combine into 16 bit values
      _tcounts = (((int16_t)_buffer[0 + _enFifoAccel*6]) << 8) | _buffer[1 + _enFifoAccel*6];
      // transform and convert to float values
      _tFifo[i] = ((((float) _tcounts) - _tempOffset)/_tempScale) + _tempOffset;
      _tSize = _fifoSize/_fifoFrameSize;
      }
      if (_enFifoGyro) {
      // combine into 16 bit values
      _gxcounts = (((int16_t)_buffer[0 + _enFifoAccel*6 + _enFifoTemp*2]) << 8) | _buffer[1 + _enFifoAccel*6 + _enFifoTemp*2];
      _gycounts = (((int16_t)_buffer[2 + _enFifoAccel*6 + _enFifoTemp*2]) << 8) | _buffer[3 + _enFifoAccel*6 + _enFifoTemp*2];
      _gzcounts = (((int16_t)_buffer[4 + _enFifoAccel*6 + _enFifoTemp*2]) << 8) | _buffer[5 + _enFifoAccel*6 + _enFifoTemp*2];
      // transform and convert to float values
      _gxFifo[i] = ((float)(tX[0]*_gxcounts + tX[1]*_gycounts + tX[2]*_gzcounts) * _gyroScale) - _gxb;
      _gyFifo[i] = ((float)(tY[0]*_gxcounts + tY[1]*_gycounts + tY[2]*_gzcounts) * _gyroScale) - _gyb;
      _gzFifo[i] = ((float)(tZ[0]*_gxcounts + tZ[1]*_gycounts + tZ[2]*_gzcounts) * _gyroScale) - _gzb;
      _gSize = _fifoSize/_fifoFrameSize;
      }
      if (_enFifoMag) {
      // combine into 16 bit values
      _hxcounts = (((int16_t)_buffer[1 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]) << 8) | _buffer[0 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6];
      _hycounts = (((int16_t)_buffer[3 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]) << 8) | _buffer[2 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6];
      _hzcounts = (((int16_t)_buffer[5 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]) << 8) | _buffer[4 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6];
      // transform and convert to float values
      _hxFifo[i] = (((float)(_hxcounts) * _magScaleX) - _hxb)*_hxs;
      _hyFifo[i] = (((float)(_hycounts) * _magScaleY) - _hyb)*_hys;
      _hzFifo[i] = (((float)(_hzcounts) * _magScaleZ) - _hzb)*_hzs;
      _hSize = _fifoSize/_fifoFrameSize;
      }
      }
      return 1;
      }
      /* returns the accelerometer FIFO size and data in the x direction, m/s/s */
      void MPU9250FIFO::getFifoAccelX_mss(size_t *size,float* data) {
      *size = _aSize;
      memcpy(data,_axFifo,_aSize*sizeof(float));
      }
      /* returns the accelerometer FIFO size and data in the y direction, m/s/s */
      void MPU9250FIFO::getFifoAccelY_mss(size_t *size,float* data) {
      *size = _aSize;
      memcpy(data,_ayFifo,_aSize*sizeof(float));
      }
      /* returns the accelerometer FIFO size and data in the z direction, m/s/s */
      void MPU9250FIFO::getFifoAccelZ_mss(size_t *size,float* data) {
      *size = _aSize;
      memcpy(data,_azFifo,_aSize*sizeof(float));
      }
      /* returns the gyroscope FIFO size and data in the x direction, rad/s */
      void MPU9250FIFO::getFifoGyroX_rads(size_t *size,float* data) {
      *size = _gSize;
      memcpy(data,_gxFifo,_gSize*sizeof(float));
      }
      /* returns the gyroscope FIFO size and data in the y direction, rad/s */
      void MPU9250FIFO::getFifoGyroY_rads(size_t *size,float* data) {
      *size = _gSize;
      memcpy(data,_gyFifo,_gSize*sizeof(float));
      }
      /* returns the gyroscope FIFO size and data in the z direction, rad/s */
      void MPU9250FIFO::getFifoGyroZ_rads(size_t *size,float* data) {
      *size = _gSize;
      memcpy(data,_gzFifo,_gSize*sizeof(float));
      }
      /* returns the magnetometer FIFO size and data in the x direction, uT */
      void MPU9250FIFO::getFifoMagX_uT(size_t *size,float* data) {
      *size = _hSize;
      memcpy(data,_hxFifo,_hSize*sizeof(float));
      }
      /* returns the magnetometer FIFO size and data in the y direction, uT */
      void MPU9250FIFO::getFifoMagY_uT(size_t *size,float* data) {
      *size = _hSize;
      memcpy(data,_hyFifo,_hSize*sizeof(float));
      }
      /* returns the magnetometer FIFO size and data in the z direction, uT */
      void MPU9250FIFO::getFifoMagZ_uT(size_t *size,float* data) {
      *size = _hSize;
      memcpy(data,_hzFifo,_hSize*sizeof(float));
      }
      /* returns the die temperature FIFO size and data, C */
      void MPU9250FIFO::getFifoTemperature_C(size_t *size,float* data) {
      *size = _tSize;
      memcpy(data,_tFifo,_tSize*sizeof(float));
      }
      /* estimates the gyro biases */
      int MPU9250::calibrateGyro() {
      // set the range, bandwidth, and srd
      if (setGyroRange(GYRO_RANGE_250DPS) < 0) {
      return -1;
      }
      if (setDlpfBandwidth(DLPF_BANDWIDTH_20HZ) < 0) {
      return -2;
      }
      if (setSrd(19) < 0) {
      return -3;
      }
      // take samples and find bias
      _gxbD = 0;
      _gybD = 0;
      _gzbD = 0;
      for (size_t i=0; i < _numSamples; i++) {
      readSensor();
      _gxbD += (getGyroX_rads() + _gxb)/((double)_numSamples);
      _gybD += (getGyroY_rads() + _gyb)/((double)_numSamples);
      _gzbD += (getGyroZ_rads() + _gzb)/((double)_numSamples);
      delay(20);
      }
      _gxb = (float)_gxbD;
      _gyb = (float)_gybD;
      _gzb = (float)_gzbD;
      // set the range, bandwidth, and srd back to what they were
      if (setGyroRange(_gyroRange) < 0) {
      return -4;
      }
      if (setDlpfBandwidth(_bandwidth) < 0) {
      return -5;
      }
      if (setSrd(_srd) < 0) {
      return -6;
      }
      return 1;
      }
      /* returns the gyro bias in the X direction, rad/s */
      float MPU9250::getGyroBiasX_rads() {
      return _gxb;
      }
      /* returns the gyro bias in the Y direction, rad/s */
      float MPU9250::getGyroBiasY_rads() {
      return _gyb;
      }
      /* returns the gyro bias in the Z direction, rad/s */
      float MPU9250::getGyroBiasZ_rads() {
      return _gzb;
      }
      /* sets the gyro bias in the X direction to bias, rad/s */
      void MPU9250::setGyroBiasX_rads(float bias) {
      _gxb = bias;
      }
      /* sets the gyro bias in the Y direction to bias, rad/s */
      void MPU9250::setGyroBiasY_rads(float bias) {
      _gyb = bias;
      }
      /* sets the gyro bias in the Z direction to bias, rad/s */
      void MPU9250::setGyroBiasZ_rads(float bias) {
      _gzb = bias;
      }
      /* finds bias and scale factor calibration for the accelerometer,
      this should be run for each axis in each direction (6 total) to find
      the min and max values along each */
      int MPU9250::calibrateAccel() {
      // set the range, bandwidth, and srd
      if (setAccelRange(ACCEL_RANGE_2G) < 0) {
      return -1;
      }
      if (setDlpfBandwidth(DLPF_BANDWIDTH_20HZ) < 0) {
      return -2;
      }
      if (setSrd(19) < 0) {
      return -3;
      }
      // take samples and find min / max
      _axbD = 0;
      _aybD = 0;
      _azbD = 0;
      for (size_t i=0; i < _numSamples; i++) {
      readSensor();
      _axbD += (getAccelX_mss()/_axs + _axb)/((double)_numSamples);
      _aybD += (getAccelY_mss()/_ays + _ayb)/((double)_numSamples);
      _azbD += (getAccelZ_mss()/_azs + _azb)/((double)_numSamples);
      delay(20);
      }
      if (_axbD > 9.0f) {
      _axmax = (float)_axbD;
      }
      if (_aybD > 9.0f) {
      _aymax = (float)_aybD;
      }
      if (_azbD > 9.0f) {
      _azmax = (float)_azbD;
      }
      if (_axbD < -9.0f) {
      _axmin = (float)_axbD;
      }
      if (_aybD < -9.0f) {
      _aymin = (float)_aybD;
      }
      if (_azbD < -9.0f) {
      _azmin = (float)_azbD;
      }
      // find bias and scale factor
      if ((abs(_axmin) > 9.0f) && (abs(_axmax) > 9.0f)) {
      _axb = (_axmin + _axmax) / 2.0f;
      _axs = G/((abs(_axmin) + abs(_axmax)) / 2.0f);
      }
      if ((abs(_aymin) > 9.0f) && (abs(_aymax) > 9.0f)) {
      _ayb = (_aymin + _aymax) / 2.0f;
      _ays = G/((abs(_aymin) + abs(_aymax)) / 2.0f);
      }
      if ((abs(_azmin) > 9.0f) && (abs(_azmax) > 9.0f)) {
      _azb = (_azmin + _azmax) / 2.0f;
      _azs = G/((abs(_azmin) + abs(_azmax)) / 2.0f);
      }
      // set the range, bandwidth, and srd back to what they were
      if (setAccelRange(_accelRange) < 0) {
      return -4;
      }
      if (setDlpfBandwidth(_bandwidth) < 0) {
      return -5;
      }
      if (setSrd(_srd) < 0) {
      return -6;
      }
      return 1;
      }
      /* returns the accelerometer bias in the X direction, m/s/s */
      float MPU9250::getAccelBiasX_mss() {
      return _axb;
      }
      /* returns the accelerometer scale factor in the X direction */
      float MPU9250::getAccelScaleFactorX() {
      return _axs;
      }
      /* returns the accelerometer bias in the Y direction, m/s/s */
      float MPU9250::getAccelBiasY_mss() {
      return _ayb;
      }
      /* returns the accelerometer scale factor in the Y direction */
      float MPU9250::getAccelScaleFactorY() {
      return _ays;
      }
      /* returns the accelerometer bias in the Z direction, m/s/s */
      float MPU9250::getAccelBiasZ_mss() {
      return _azb;
      }
      /* returns the accelerometer scale factor in the Z direction */
      float MPU9250::getAccelScaleFactorZ() {
      return _azs;
      }
      /* sets the accelerometer bias (m/s/s) and scale factor in the X direction */
      void MPU9250::setAccelCalX(float bias,float scaleFactor) {
      _axb = bias;
      _axs = scaleFactor;
      }
      /* sets the accelerometer bias (m/s/s) and scale factor in the Y direction */
      void MPU9250::setAccelCalY(float bias,float scaleFactor) {
      _ayb = bias;
      _ays = scaleFactor;
      }
      /* sets the accelerometer bias (m/s/s) and scale factor in the Z direction */
      void MPU9250::setAccelCalZ(float bias,float scaleFactor) {
      _azb = bias;
      _azs = scaleFactor;
      }
      /* finds bias and scale factor calibration for the magnetometer,
      the sensor should be rotated in a figure 8 motion until complete */
      int MPU9250::calibrateMag() {
      // set the srd
      if (setSrd(19) < 0) {
      return -1;
      }
      // get a starting set of data
      readSensor();
      _hxmax = getMagX_uT();
      _hxmin = getMagX_uT();
      _hymax = getMagY_uT();
      _hymin = getMagY_uT();
      _hzmax = getMagZ_uT();
      _hzmin = getMagZ_uT();
      // collect data to find max / min in each channel
      _counter = 0;
      while (_counter < _maxCounts) {
      _delta = 0.0f;
      _framedelta = 0.0f;
      readSensor();
      _hxfilt = (_hxfilt*((float)_coeff-1)+(getMagX_uT()/_hxs+_hxb))/((float)_coeff);
      _hyfilt = (_hyfilt*((float)_coeff-1)+(getMagY_uT()/_hys+_hyb))/((float)_coeff);
      _hzfilt = (_hzfilt*((float)_coeff-1)+(getMagZ_uT()/_hzs+_hzb))/((float)_coeff);
      if (_hxfilt > _hxmax) {
      _delta = _hxfilt - _hxmax;
      _hxmax = _hxfilt;
      }
      if (_delta > _framedelta) {
      _framedelta = _delta;
      }
      if (_hyfilt > _hymax) {
      _delta = _hyfilt - _hymax;
      _hymax = _hyfilt;
      }
      if (_delta > _framedelta) {
      _framedelta = _delta;
      }
      if (_hzfilt > _hzmax) {
      _delta = _hzfilt - _hzmax;
      _hzmax = _hzfilt;
      }
      if (_delta > _framedelta) {
      _framedelta = _delta;
      }
      if (_hxfilt < _hxmin) {
      _delta = abs(_hxfilt - _hxmin);
      _hxmin = _hxfilt;
      }
      if (_delta > _framedelta) {
      _framedelta = _delta;
      }
      if (_hyfilt < _hymin) {
      _delta = abs(_hyfilt - _hymin);
      _hymin = _hyfilt;
      }
      if (_delta > _framedelta) {
      _framedelta = _delta;
      }
      if (_hzfilt < _hzmin) {
      _delta = abs(_hzfilt - _hzmin);
      _hzmin = _hzfilt;
      }
      if (_delta > _framedelta) {
      _framedelta = _delta;
      }
      if (_framedelta > _deltaThresh) {
      _counter = 0;
      } else {
      _counter++;
      }
      delay(20);
      }
      // find the magnetometer bias
      _hxb = (_hxmax + _hxmin) / 2.0f;
      _hyb = (_hymax + _hymin) / 2.0f;
      _hzb = (_hzmax + _hzmin) / 2.0f;
      // find the magnetometer scale factor
      _hxs = (_hxmax - _hxmin) / 2.0f;
      _hys = (_hymax - _hymin) / 2.0f;
      _hzs = (_hzmax - _hzmin) / 2.0f;
      _avgs = (_hxs + _hys + _hzs) / 3.0f;
      _hxs = _avgs/_hxs;
      _hys = _avgs/_hys;
      _hzs = _avgs/_hzs;
      // set the srd back to what it was
      if (setSrd(_srd) < 0) {
      return -2;
      }
      return 1;
      }
      /* returns the magnetometer bias in the X direction, uT */
      float MPU9250::getMagBiasX_uT() {
      return _hxb;
      }
      /* returns the magnetometer scale factor in the X direction */
      float MPU9250::getMagScaleFactorX() {
      return _hxs;
      }
      /* returns the magnetometer bias in the Y direction, uT */
      float MPU9250::getMagBiasY_uT() {
      return _hyb;
      }
      /* returns the magnetometer scale factor in the Y direction */
      float MPU9250::getMagScaleFactorY() {
      return _hys;
      }
      /* returns the magnetometer bias in the Z direction, uT */
      float MPU9250::getMagBiasZ_uT() {
      return _hzb;
      }
      /* returns the magnetometer scale factor in the Z direction */
      float MPU9250::getMagScaleFactorZ() {
      return _hzs;
      }
      /* sets the magnetometer bias (uT) and scale factor in the X direction */
      void MPU9250::setMagCalX(float bias,float scaleFactor) {
      _hxb = bias;
      _hxs = scaleFactor;
      }
      /* sets the magnetometer bias (uT) and scale factor in the Y direction */
      void MPU9250::setMagCalY(float bias,float scaleFactor) {
      _hyb = bias;
      _hys = scaleFactor;
      }
      /* sets the magnetometer bias (uT) and scale factor in the Z direction */
      void MPU9250::setMagCalZ(float bias,float scaleFactor) {
      _hzb = bias;
      _hzs = scaleFactor;
      }
      /* writes a byte to MPU9250 register given a register address and data */
      int MPU9250::writeRegister(uint8_t subAddress, uint8_t data){
      /* write data to device */
      if( _useSPI ){
      _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction
      digitalWrite(_csPin,LOW); // select the MPU9250 chip
      _spi->transfer(subAddress); // write the register address
      _spi->transfer(data); // write the data
      digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
      _spi->endTransaction(); // end the transaction
      }
      else{
      _i2c->beginTransmission(_address); // open the device
      _i2c->write(subAddress); // write the register address
      _i2c->write(data); // write the data
      _i2c->endTransmission();
      }
      delay(10);
      /* read back the register */
      readRegisters(subAddress,1,_buffer);
      /* check the read back register against the written register */
      if(_buffer[0] == data) {
      return 1;
      }
      else{
      return -1;
      }
      }
      /* reads registers from MPU9250 given a starting register address, number of bytes, and a pointer to store data */
      int MPU9250::readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest){
      if( _useSPI ){
      // begin the transaction
      if(_useSPIHS){
      _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3));
      }
      else{
      _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3));
      }
      digitalWrite(_csPin,LOW); // select the MPU9250 chip
      _spi->transfer(subAddress | SPI_READ); // specify the starting register address
      for(uint8_t i = 0; i < count; i++){
      dest[i] = _spi->transfer(0x00); // read the data
      }
      digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
      _spi->endTransaction(); // end the transaction
      return 1;
      }
      else{
      _i2c->beginTransmission(_address); // open the device
      _i2c->write(subAddress); // specify the starting register address
      _i2c->endTransmission(false);
      _numBytes = _i2c->requestFrom(_address, count); // specify the number of bytes to receive
      if (_numBytes == count) {
      for(uint8_t i = 0; i < count; i++){
      dest[i] = _i2c->read();
      }
      return 1;
      } else {
      return -1;
      }
      }
      }
      /* writes a register to the AK8963 given a register address and data */
      int MPU9250::writeAK8963Register(uint8_t subAddress, uint8_t data){
      // set slave 0 to the AK8963 and set for write
      if (writeRegister(I2C_SLV0_ADDR,AK8963_I2C_ADDR) < 0) {
      return -1;
      }
      // set the register to the desired AK8963 sub address
      if (writeRegister(I2C_SLV0_REG,subAddress) < 0) {
      return -2;
      }
      // store the data for write
      if (writeRegister(I2C_SLV0_DO,data) < 0) {
      return -3;
      }
      // enable I2C and send 1 byte
      if (writeRegister(I2C_SLV0_CTRL,I2C_SLV0_EN | (uint8_t)1) < 0) {
      return -4;
      }
      // read the register and confirm
      if (readAK8963Registers(subAddress,1,_buffer) < 0) {
      return -5;
      }
      if(_buffer[0] == data) {
      return 1;
      } else{
      return -6;
      }
      }
      /* reads registers from the AK8963 */
      int MPU9250::readAK8963Registers(uint8_t subAddress, uint8_t count, uint8_t* dest){
      // set slave 0 to the AK8963 and set for read
      if (writeRegister(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG) < 0) {
      return -1;
      }
      // set the register to the desired AK8963 sub address
      if (writeRegister(I2C_SLV0_REG,subAddress) < 0) {
      return -2;
      }
      // enable I2C and request the bytes
      if (writeRegister(I2C_SLV0_CTRL,I2C_SLV0_EN | count) < 0) {
      return -3;
      }
      delay(1); // takes some time for these registers to fill
      // read the bytes off the MPU9250 EXT_SENS_DATA registers
      _status = readRegisters(EXT_SENS_DATA_00,count,dest);
      return _status;
      }
      /* gets the MPU9250 WHO_AM_I register value, expected to be 0x71 */
      int MPU9250::whoAmI(){
      // read the WHO AM I register
      if (readRegisters(WHO_AM_I,1,_buffer) < 0) {
      return -1;
      }
      // return the register value
      return _buffer[0];
      }
      /* gets the AK8963 WHO_AM_I register value, expected to be 0x48 */
      int MPU9250::whoAmIAK8963(){
      // read the WHO AM I register
      if (readAK8963Registers(AK8963_WHO_AM_I,1,_buffer) < 0) {
      return -1;
      }
      // return the register value
      return _buffer[0];
      }
      MPU9250.h 0 → 100644
      View file @ 097a5bd4
      /*
      MPU9250.h
      Brian R Taylor
      brian.taylor@bolderflight.com
      Copyright (c) 2017 Bolder Flight Systems
      Permission is hereby granted, free of charge, to any person obtaining a copy of this software
      and associated documentation files (the "Software"), to deal in the Software without restriction,
      including without limitation the rights to use, copy, modify, merge, publish, distribute,
      sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
      furnished to do so, subject to the following conditions:
      The above copyright notice and this permission notice shall be included in all copies or
      substantial portions of the Software.
      THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
      BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
      NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
      DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
      OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
      */
      #ifndef MPU9250_h
      #define MPU9250_h
      #include "Arduino.h"
      #include "Wire.h" // I2C library
      #include "SPI.h" // SPI library
      class MPU9250{
      public:
      enum GyroRange
      {
      GYRO_RANGE_250DPS,
      GYRO_RANGE_500DPS,
      GYRO_RANGE_1000DPS,
      GYRO_RANGE_2000DPS
      };
      enum AccelRange
      {
      ACCEL_RANGE_2G,
      ACCEL_RANGE_4G,
      ACCEL_RANGE_8G,
      ACCEL_RANGE_16G
      };
      enum DlpfBandwidth
      {
      DLPF_BANDWIDTH_184HZ,
      DLPF_BANDWIDTH_92HZ,
      DLPF_BANDWIDTH_41HZ,
      DLPF_BANDWIDTH_20HZ,
      DLPF_BANDWIDTH_10HZ,
      DLPF_BANDWIDTH_5HZ
      };
      enum LpAccelOdr
      {
      LP_ACCEL_ODR_0_24HZ = 0,
      LP_ACCEL_ODR_0_49HZ = 1,
      LP_ACCEL_ODR_0_98HZ = 2,
      LP_ACCEL_ODR_1_95HZ = 3,
      LP_ACCEL_ODR_3_91HZ = 4,
      LP_ACCEL_ODR_7_81HZ = 5,
      LP_ACCEL_ODR_15_63HZ = 6,
      LP_ACCEL_ODR_31_25HZ = 7,
      LP_ACCEL_ODR_62_50HZ = 8,
      LP_ACCEL_ODR_125HZ = 9,
      LP_ACCEL_ODR_250HZ = 10,
      LP_ACCEL_ODR_500HZ = 11
      };
      MPU9250(TwoWire &bus,uint8_t address);
      MPU9250(SPIClass &bus,uint8_t csPin);
      int begin();
      int setAccelRange(AccelRange range);
      int setGyroRange(GyroRange range);
      int setDlpfBandwidth(DlpfBandwidth bandwidth);
      int setSrd(uint8_t srd);
      int enableDataReadyInterrupt();
      int disableDataReadyInterrupt();
      int enableWakeOnMotion(float womThresh_mg,LpAccelOdr odr);
      int readSensor();
      float getAccelX_mss();
      float getAccelY_mss();
      float getAccelZ_mss();
      float getGyroX_rads();
      float getGyroY_rads();
      float getGyroZ_rads();
      float getMagX_uT();
      float getMagY_uT();
      float getMagZ_uT();
      float getTemperature_C();
      int calibrateGyro();
      float getGyroBiasX_rads();
      float getGyroBiasY_rads();
      float getGyroBiasZ_rads();
      void setGyroBiasX_rads(float bias);
      void setGyroBiasY_rads(float bias);
      void setGyroBiasZ_rads(float bias);
      int calibrateAccel();
      float getAccelBiasX_mss();
      float getAccelScaleFactorX();
      float getAccelBiasY_mss();
      float getAccelScaleFactorY();
      float getAccelBiasZ_mss();
      float getAccelScaleFactorZ();
      void setAccelCalX(float bias,float scaleFactor);
      void setAccelCalY(float bias,float scaleFactor);
      void setAccelCalZ(float bias,float scaleFactor);
      int calibrateMag();
      float getMagBiasX_uT();
      float getMagScaleFactorX();
      float getMagBiasY_uT();
      float getMagScaleFactorY();
      float getMagBiasZ_uT();
      float getMagScaleFactorZ();
      void setMagCalX(float bias,float scaleFactor);
      void setMagCalY(float bias,float scaleFactor);
      void setMagCalZ(float bias,float scaleFactor);
      protected:
      // i2c
      uint8_t _address;
      TwoWire *_i2c;
      const uint32_t _i2cRate = 400000; // 400 kHz
      size_t _numBytes; // number of bytes received from I2C
      // spi
      SPIClass *_spi;
      uint8_t _csPin;
      bool _useSPI;
      bool _useSPIHS;
      const uint8_t SPI_READ = 0x80;
      const uint32_t SPI_LS_CLOCK = 1000000; // 1 MHz
      const uint32_t SPI_HS_CLOCK = 15000000; // 15 MHz
      // track success of interacting with sensor
      int _status;
      // buffer for reading from sensor
      uint8_t _buffer[21];
      // data counts
      int16_t _axcounts,_aycounts,_azcounts;
      int16_t _gxcounts,_gycounts,_gzcounts;
      int16_t _hxcounts,_hycounts,_hzcounts;
      int16_t _tcounts;
      // data buffer
      float _ax, _ay, _az;
      float _gx, _gy, _gz;
      float _hx, _hy, _hz;
      float _t;
      // wake on motion
      uint8_t _womThreshold;
      // scale factors
      float _accelScale;
      float _gyroScale;
      float _magScaleX, _magScaleY, _magScaleZ;
      const float _tempScale = 333.87f;
      const float _tempOffset = 21.0f;
      // configuration
      AccelRange _accelRange;
      GyroRange _gyroRange;
      DlpfBandwidth _bandwidth;
      uint8_t _srd;
      // gyro bias estimation
      size_t _numSamples = 100;
      double _gxbD, _gybD, _gzbD;
      float _gxb, _gyb, _gzb;
      // accel bias and scale factor estimation
      double _axbD, _aybD, _azbD;
      float _axmax, _aymax, _azmax;
      float _axmin, _aymin, _azmin;
      float _axb, _ayb, _azb;
      float _axs = 1.0f;
      float _ays = 1.0f;
      float _azs = 1.0f;
      // magnetometer bias and scale factor estimation
      uint16_t _maxCounts = 1000;
      float _deltaThresh = 0.3f;
      uint8_t _coeff = 8;
      uint16_t _counter;
      float _framedelta, _delta;
      float _hxfilt, _hyfilt, _hzfilt;
      float _hxmax, _hymax, _hzmax;
      float _hxmin, _hymin, _hzmin;
      float _hxb, _hyb, _hzb;
      float _hxs = 1.0f;
      float _hys = 1.0f;
      float _hzs = 1.0f;
      float _avgs;
      // transformation matrix
      /* transform the accel and gyro axes to match the magnetometer axes */
      const int16_t tX[3] = {0, 1, 0};
      const int16_t tY[3] = {1, 0, 0};
      const int16_t tZ[3] = {0, 0, -1};
      // constants
      const float G = 9.807f;
      const float _d2r = 3.14159265359f/180.0f;
      // MPU9250 registers
      const uint8_t ACCEL_OUT = 0x3B;
      const uint8_t GYRO_OUT = 0x43;
      const uint8_t TEMP_OUT = 0x41;
      const uint8_t EXT_SENS_DATA_00 = 0x49;
      const uint8_t ACCEL_CONFIG = 0x1C;
      const uint8_t ACCEL_FS_SEL_2G = 0x00;
      const uint8_t ACCEL_FS_SEL_4G = 0x08;
      const uint8_t ACCEL_FS_SEL_8G = 0x10;
      const uint8_t ACCEL_FS_SEL_16G = 0x18;
      const uint8_t GYRO_CONFIG = 0x1B;
      const uint8_t GYRO_FS_SEL_250DPS = 0x00;
      const uint8_t GYRO_FS_SEL_500DPS = 0x08;
      const uint8_t GYRO_FS_SEL_1000DPS = 0x10;
      const uint8_t GYRO_FS_SEL_2000DPS = 0x18;
      const uint8_t ACCEL_CONFIG2 = 0x1D;
      const uint8_t ACCEL_DLPF_184 = 0x01;
      const uint8_t ACCEL_DLPF_92 = 0x02;
      const uint8_t ACCEL_DLPF_41 = 0x03;
      const uint8_t ACCEL_DLPF_20 = 0x04;
      const uint8_t ACCEL_DLPF_10 = 0x05;
      const uint8_t ACCEL_DLPF_5 = 0x06;
      const uint8_t CONFIG = 0x1A;
      const uint8_t GYRO_DLPF_184 = 0x01;
      const uint8_t GYRO_DLPF_92 = 0x02;
      const uint8_t GYRO_DLPF_41 = 0x03;
      const uint8_t GYRO_DLPF_20 = 0x04;
      const uint8_t GYRO_DLPF_10 = 0x05;
      const uint8_t GYRO_DLPF_5 = 0x06;
      const uint8_t SMPDIV = 0x19;
      const uint8_t INT_PIN_CFG = 0x37;
      const uint8_t INT_ENABLE = 0x38;
      const uint8_t INT_DISABLE = 0x00;
      const uint8_t INT_PULSE_50US = 0x00;
      const uint8_t INT_WOM_EN = 0x40;
      const uint8_t INT_RAW_RDY_EN = 0x01;
      const uint8_t PWR_MGMNT_1 = 0x6B;
      const uint8_t PWR_CYCLE = 0x20;
      const uint8_t PWR_RESET = 0x80;
      const uint8_t CLOCK_SEL_PLL = 0x01;
      const uint8_t PWR_MGMNT_2 = 0x6C;
      const uint8_t SEN_ENABLE = 0x00;
      const uint8_t DIS_GYRO = 0x07;
      const uint8_t USER_CTRL = 0x6A;
      const uint8_t I2C_MST_EN = 0x20;
      const uint8_t I2C_MST_CLK = 0x0D;
      const uint8_t I2C_MST_CTRL = 0x24;
      const uint8_t I2C_SLV0_ADDR = 0x25;
      const uint8_t I2C_SLV0_REG = 0x26;
      const uint8_t I2C_SLV0_DO = 0x63;
      const uint8_t I2C_SLV0_CTRL = 0x27;
      const uint8_t I2C_SLV0_EN = 0x80;
      const uint8_t I2C_READ_FLAG = 0x80;
      const uint8_t MOT_DETECT_CTRL = 0x69;
      const uint8_t ACCEL_INTEL_EN = 0x80;
      const uint8_t ACCEL_INTEL_MODE = 0x40;
      const uint8_t LP_ACCEL_ODR = 0x1E;
      const uint8_t WOM_THR = 0x1F;
      const uint8_t WHO_AM_I = 0x75;
      const uint8_t FIFO_EN = 0x23;
      const uint8_t FIFO_TEMP = 0x80;
      const uint8_t FIFO_GYRO = 0x70;
      const uint8_t FIFO_ACCEL = 0x08;
      const uint8_t FIFO_MAG = 0x01;
      const uint8_t FIFO_COUNT = 0x72;
      const uint8_t FIFO_READ = 0x74;
      // AK8963 registers
      const uint8_t AK8963_I2C_ADDR = 0x0C;
      const uint8_t AK8963_HXL = 0x03;
      const uint8_t AK8963_CNTL1 = 0x0A;
      const uint8_t AK8963_PWR_DOWN = 0x00;
      const uint8_t AK8963_CNT_MEAS1 = 0x12;
      const uint8_t AK8963_CNT_MEAS2 = 0x16;
      const uint8_t AK8963_FUSE_ROM = 0x0F;
      const uint8_t AK8963_CNTL2 = 0x0B;
      const uint8_t AK8963_RESET = 0x01;
      const uint8_t AK8963_ASA = 0x10;
      const uint8_t AK8963_WHO_AM_I = 0x00;
      // private functions
      int writeRegister(uint8_t subAddress, uint8_t data);
      int readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest);
      int writeAK8963Register(uint8_t subAddress, uint8_t data);
      int readAK8963Registers(uint8_t subAddress, uint8_t count, uint8_t* dest);
      int whoAmI();
      int whoAmIAK8963();
      };
      class MPU9250FIFO: public MPU9250 {
      public:
      using MPU9250::MPU9250;
      int enableFifo(bool accel,bool gyro,bool mag,bool temp);
      int readFifo();
      void getFifoAccelX_mss(size_t *size,float* data);
      void getFifoAccelY_mss(size_t *size,float* data);
      void getFifoAccelZ_mss(size_t *size,float* data);
      void getFifoGyroX_rads(size_t *size,float* data);
      void getFifoGyroY_rads(size_t *size,float* data);
      void getFifoGyroZ_rads(size_t *size,float* data);
      void getFifoMagX_uT(size_t *size,float* data);
      void getFifoMagY_uT(size_t *size,float* data);
      void getFifoMagZ_uT(size_t *size,float* data);
      void getFifoTemperature_C(size_t *size,float* data);
      protected:
      // fifo
      bool _enFifoAccel,_enFifoGyro,_enFifoMag,_enFifoTemp;
      size_t _fifoSize,_fifoFrameSize;
      float _axFifo[85], _ayFifo[85], _azFifo[85];
      size_t _aSize;
      float _gxFifo[85], _gyFifo[85], _gzFifo[85];
      size_t _gSize;
      float _hxFifo[73], _hyFifo[73], _hzFifo[73];
      size_t _hSize;
      float _tFifo[256];
      size_t _tSize;
      };
      #endif
      README.md 0 → 100644
      View file @ 097a5bd4
      ## 배선
      ### MPU-9250 연결
      | MPU9250 | Arduino Nano |
      |:-------:|:------------:|
      | VCC | 5V |
      | GND | GND |
      | SCL | A5 |
      | SDA | A4 |
      ### HC-06 연결
      | HC-06 | Arduino Nano |
      |:-----:|:------------:|
      | VCC | 5V |
      | GND | GND |
      | RXD | 3 |
      | TXD | 2 |
      ### 조이스틱 연결
      | Joystick | Arduino Nano |
      |:--------:|:------------:|
      | +5V | 5V |
      | GND | GND |
      | VRx | A2 |
      | VRy | A1 |
      | SW | 5 |
      ### 배터리 연결
      | Battery | Arduino Nano |
      |:---------:|:------------:|
      | + | 5V |
      | - | GND |
      \ No newline at end of file
      controller.ino 0 → 100644
      View file @ 097a5bd4
      #include <SoftwareSerial.h>
      #include "MPU9250.h"
      const int btRx = 3, btTx = 2, sw = 5;
      const int x = A1, y = A2;
      SoftwareSerial bt(btTx, btRx);
      MPU9250 IMU(Wire,0x68);
      int status, xx, yy, swsw;
      void setup()
      {
      Serial.begin(9600);
      pinMode(x, INPUT);
      pinMode(y, INPUT);
      pinMode(sw, INPUT);
      digitalWrite(sw, HIGH);
      while(!Serial) {}
      bt.begin(9600);
      bt.println("AT+NAMEdronermctrl");
      status = IMU.begin();
      if (status < 0)
      {
      Serial.print("IMU Error: ");
      Serial.println(status);
      while(1) { }
      }
      }
      void writeulong(unsigned long x)
      {
      Serial.print(x, HEX);
      Serial.print(" ");
      bt.print(x, HEX);
      bt.print(" ");
      }
      void writefloat(float x)
      {
      byte* ptr = (byte*)&x;
      unsigned long v = ((unsigned long)ptr[3] << 24) | ((unsigned long)ptr[2] << 16) | ((unsigned long)ptr[1] << 8) | ptr[0];
      Serial.print(v, HEX);
      Serial.print(" ");
      bt.print(v, HEX);
      bt.print(" ");
      }
      void loop()
      {
      xx = analogRead(x);
      yy = analogRead(y);
      swsw = digitalRead(sw);
      IMU.readSensor();
      Serial.print("=");
      bt.print("=");
      writeulong(micros());
      writefloat(IMU.getAccelX_mss());
      writefloat(IMU.getAccelY_mss());
      writefloat(IMU.getAccelZ_mss());
      writefloat(IMU.getGyroX_rads());
      writefloat(IMU.getGyroY_rads());
      writefloat(IMU.getGyroZ_rads());
      Serial.print(xx);
      Serial.print(" ");
      Serial.print(yy);
      Serial.print(" ");
      Serial.print(swsw);
      bt.print(xx);
      bt.print(" ");
      bt.print(yy);
      bt.print(" ");
      bt.print(swsw);
      Serial.println();
      bt.println();
      delay(100);
      }
      Markdown is supported
      0% or
      You are about to add 0 people to the discussion. Proceed with caution.
      Finish editing this message first!
      Please register or to comment