wuhyuo~~ koitsuwa sugoi~~ cho lucky-dazegit config user.name dlarudgus20

sugoi/Integrator.cpp

+#include "Integrator.h"
+#include "Quaternion.h"
+#include "Arduino.h"
+
+Integrator::Integrator(float initial /* = 0 */)
+    : value_(initial), speed_(0), prev_(0)
+{
+}
+
+float Integrator::value() const { return value_; }
+float Integrator::speed() const { return speed_; }
+
+float Integrator::accumulate(float accel, float dt)
+{
+    float v = speed_ + accel * dt / 2;
+    float dx = v * dt;
+    value_ += dx;
+    speed_ += accel * dt;
+    return dx;
+}
+
+INS::INS()
+    : gx_(0), gy_(0), gz_(0)
+{
+}
+
+float INS::posX() const { return posX_.value(); }
+float INS::posY() const { return posY_.value(); }
+float INS::posZ() const { return posZ_.value(); }
+float INS::rotX() const { return rotX_.value(); }
+float INS::rotY() const { return rotY_.value(); }
+float INS::rotZ() const { return rotZ_.value(); }
+
+float INS::posDX() const { return posX_.speed(); }
+float INS::posDY() const { return posY_.speed(); }
+float INS::posDZ() const { return posZ_.speed(); }
+float INS::rotDX() const { return rotX_.speed(); }
+float INS::rotDY() const { return rotY_.speed(); }
+float INS::rotDZ() const { return rotZ_.speed(); }
+
+float INS::grvX() const { return gx_; }
+float INS::grvY() const { return gy_; }
+float INS::grvZ() const { return gz_; }
+
+void INS::setGravity(float ax, float ay, float az)
+{
+    gx_ = ax;
+    gy_ = ay;
+    gz_ = az;
+}
+
+void INS::accumulate(float px, float py, float pz,
+    float rx, float ry, float rz, float dt)
+{
+    float rdx = rotX_.accumulate(rx, dt);
+    float rdy = rotY_.accumulate(ry, dt);
+    float rdz = rotZ_.accumulate(rz, dt);
+
+    Quaternion q = Quaternion::from_euler_rotation(rdx, rdy, rdz);
+    Quaternion g(gx_, gy_, gz_);
+    g = q.rotate(g);
+    gx_ = g.b;
+    gy_ = g.c;
+    gz_ = g.d;
+
+    posX_.accumulate(px - gx_, dt);
+    posY_.accumulate(py - gy_, dt);
+    posZ_.accumulate(pz - gz_, dt);
+}

sugoi/Integrator.h

+#ifndef INTEGRATOR_H_
+#define INTEGRATOR_H_
+
+class Integrator
+{
+public:
+    explicit Integrator(float initial = 0);
+
+    float value() const;
+    float speed() const;
+    float accumulate(float accel, float dt);
+
+private:
+    float prev_;
+    float speed_;
+    float value_;
+};
+
+class INS
+{
+public:
+    INS();
+
+    float posX() const;
+    float posY() const;
+    float posZ() const;
+    float rotX() const;
+    float rotY() const;
+    float rotZ() const;
+
+    float posDX() const;
+    float posDY() const;
+    float posDZ() const;
+    float rotDX() const;
+    float rotDY() const;
+    float rotDZ() const;
+
+    float grvX() const;
+    float grvY() const;
+    float grvZ() const;
+
+    void setGravity(float ax, float ay, float az);
+
+    void accumulate(float px, float py, float pz,
+        float rx, float ry, float rz, float dt);
+
+private:
+    float gx_, gy_, gz_;
+    Integrator posX_, posY_, posZ_;
+    Integrator rotX_, rotY_, rotZ_;
+};
+
+#endif // INTEGRATOR_H_

sugoi/MPU9250.cpp

+/*
+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];
+}

sugoi/MPU9250.h

+/*
+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

sugoi/Quaternion.cpp

+#include "Quaternion.h"
+#include "Arduino.h"
+
+// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/arithmetic/index.htm
+// 800B
+Quaternion & Quaternion::operator*=(const Quaternion &q) {
+    Quaternion ret;
+    ret.a = a*q.a - b*q.b - c*q.c - d*q.d;
+    ret.b = b*q.a + a*q.b + c*q.d - d*q.c;
+    ret.c = a*q.c - b*q.d + c*q.a + d*q.b;
+    ret.d = a*q.d + b*q.c - c*q.b + d*q.a;
+    return (*this = ret);
+}
+
+Quaternion & Quaternion::operator+=(const Quaternion &q) {
+    a += q.a;
+    b += q.b;
+    c += q.c;
+    d += q.d;
+    return *this;
+}
+
+Quaternion & Quaternion::operator*=(float scale) {
+    a *= scale;
+    b *= scale;
+    c *= scale;
+    d *= scale;
+    return *this;
+}
+
+float Quaternion::norm() const {
+    float norm2 = a*a + b*b + c*c + d*d;
+    return sqrt(norm2);
+}
+
+// 400B
+Quaternion & Quaternion::normalize() {
+    float n = norm();
+    a /= n;
+    b /= n;
+    c /= n;
+    d /= n;
+    return *this;
+}
+
+// This method takes an euler rotation in rad and converts it to an equivilent 
+// Quaternion rotation.
+// 800B
+const Quaternion Quaternion::from_euler_rotation(float x, float y, float z) {
+    float c1 = cos(y/2);
+    float c2 = cos(z/2);
+    float c3 = cos(x/2);
+
+    float s1 = sin(y/2);
+    float s2 = sin(z/2);
+    float s3 = sin(x/2);
+    Quaternion ret;
+    ret.a = c1 * c2 * c3 - s1 * s2 * s3;
+    ret.b = s1 * s2 * c3 + c1 * c2 * s3;
+    ret.c = s1 * c2 * c3 + c1 * s2 * s3;
+    ret.d = c1 * s2 * c3 - s1 * c2 * s3;
+    return ret;
+}
+
+const Quaternion Quaternion::from_euler_rotation_approx(float x, float y, float z) {
+    // approximage cos(theta) as 1 - theta^2 / 2
+    float c1 = 1 - (y * y / 8);
+    float c2 = 1 - (z * z / 8);
+    float c3 = 1 - (x * x / 8);
+
+    // appromixate sin(theta) as theta
+    float s1 = y/2;
+    float s2 = z/2;
+    float s3 = x/2;
+    Quaternion ret;
+    ret.a = c1 * c2 * c3 - s1 * s2 * s3;
+    ret.b = s1 * s2 * c3 + c1 * c2 * s3;
+    ret.c = s1 * c2 * c3 + c1 * s2 * s3;
+    ret.d = c1 * s2 * c3 - s1 * c2 * s3;
+    return ret;
+}
+
+const Quaternion Quaternion::conj() const {
+    Quaternion ret(*this);
+    ret.b *= -1;
+    ret.c *= -1;
+    ret.d *= -1;
+    return ret;
+}
+
+// This method takes two vectors and computes the rotation vector between them.
+// Both the left and right hand sides must be pure vectors (a == 0)
+// Both the left and right hand sides must normalized already.
+// This computes the rotation that will tranform this to q.
+// 500B
+const Quaternion Quaternion::rotation_between_vectors(const Quaternion& q) const {
+    // http://www.euclideanspace.com/maths/algebra/vectors/angleBetween/
+    // We want to compute the below values.
+    // w = 1 + v1•v2
+    // x = (v1 x v2).x 
+    // y = (v1 x v2).y
+    // z = (v1 x v2).z
+
+    // Instead of writing the below code direclty, we reduce code size by
+    // just using multiplication to implement it.
+    //Quaternion ret;
+    //ret.a = 1 + b * q.b + c * q.c + d * q.d;
+    //ret.b = c * q.d - d * q.c;
+    //ret.c = d * q.b - b * q.d;
+    //ret.d = b * q.c - c * q.b;
+    //ret.normalize();
+    //return ret;
+
+    // From wikipedia https://en.wikipedia.org/wiki/Quaternion#Quaternions_and_the_geometry_of_R3
+    // The cross product p x q is just the vector part of multiplying p * q
+    Quaternion ret = (*this) * q;
+    ret.a = 1 - ret.a;
+    ret.normalize();
+    return ret;
+}
+
+float Quaternion::dot_product(const Quaternion& q) const {
+    return a * q.a + b * q.b + c * q.c + d * q.d;
+}
+
+// This will roate the input vector by this normalized rotation quaternion.
+const Quaternion Quaternion::rotate(const Quaternion& q) const {
+    return (*this) * q * conj();
+}
+
+// This modifies this normalized rotation quaternion and makes it 
+// rotate between 0-1 as much as it would normally rotate.
+// The math here is pretty sloppy but should work for 
+// most cases.
+Quaternion & Quaternion::fractional(float f) {
+    a = 1-f + f*a;
+    b *= f;
+    c *= f;
+    d *= f;
+    return normalize();
+}

sugoi/Quaternion.h

+#ifndef QUATERNION_H
+#define QUATERNION_H
+
+class Quaternion {
+public:
+    float a;
+    float b;
+    float c;
+    float d;
+
+    Quaternion() {a = 1; b = c = d = 0;}
+
+    // This is a vector that can be rotated in Quaternion space.
+    Quaternion(float x, float y, float z) {a = 0; b = x; c = y; d = z;}
+
+    // This returns a Quaternion that rotates in each given axis in radians.
+    // We use standard right hand rule for rotations and coordinates.
+    static const Quaternion from_euler_rotation(float x, float y, float z);
+
+    // This is like from_euler_rotation but for small angles (less than 45 deg (PI/4))
+    static const Quaternion from_euler_rotation_approx(float x, float y, float z);
+
+    Quaternion & operator=(const Quaternion &rhs) {
+        a = rhs.a;
+        b = rhs.b;
+        c = rhs.c;
+        d = rhs.d;
+        return *this;
+    }
+
+    // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/arithmetic/index.htm
+    Quaternion & operator*=(const Quaternion &q);
+    const Quaternion operator* (const Quaternion& q) const { return Quaternion(*this) *= q; }
+    Quaternion & operator+=(const Quaternion &q);
+    const Quaternion operator+(const Quaternion& q) const { return Quaternion(*this) += q; }
+    Quaternion & operator*=(float scale);
+    const Quaternion operator*(float scale) const { return Quaternion(*this) *= scale; }
+    float norm() const;
+    Quaternion & normalize();
+    const Quaternion conj() const;
+    // This method takes two vectors and computes the rotation vector between them.
+    // Both the left and right hand sides must be pure vectors (a == 0)
+    // Both the left and right hand sides must normalized already.
+    // This computes the rotation that will tranform this to q.
+    const Quaternion rotation_between_vectors(const Quaternion& v) const;
+    float dot_product(const Quaternion& q) const;
+
+    // This method takes one vector and rotates it using this Quaternion.
+    // The input must be a pure vector (a == 0)
+    const Quaternion rotate(const Quaternion& q) const;
+    Quaternion & fractional(float f);
+};
+
+#endif
\ No newline at end of file

sugoi/sugoi.ino

+#include "MPU9250.h"
+#include "Integrator.h"
+
+MPU9250 imu(Wire, 0x68);
+INS ins;
+unsigned long prevTime;
+
+void setup()
+{
+  Serial.begin(115200);
+
+  int status = imu.begin();
+  if (status < 0)
+  {
+    Serial.print("IMU Error: ");
+    Serial.println(status);
+    while (1) { }
+  }
+
+  imu.readSensor();
+  ins.setGravity(imu.getAccelX_mss(), imu.getAccelY_mss(), imu.getAccelZ_mss());
+}
+
+void loop()
+{
+  unsigned long nowTime = micros();
+  unsigned long deltaTime = nowTime - prevTime;
+  float dt = deltaTime / 1000000.0f;
+
+  imu.readSensor();
+  ins.accumulate(imu.getAccelX_mss(), imu.getAccelY_mss(), imu.getAccelZ_mss(),
+    imu.getGyroX_rads(), imu.getGyroY_rads(), imu.getGyroZ_rads(), dt);
+
+  if (millis() % 200 == 0)
+  {
+    Serial.print("dt : ");
+    Serial.print(deltaTime);
+    Serial.print("\t");
+    Serial.println(dt, 10);
+    Serial.print("ins :\t");
+    Serial.print(ins.posX());
+    Serial.print("\t");
+    Serial.print(ins.posY());
+    Serial.print("\t");
+    Serial.print(ins.posZ());
+    Serial.print("\t");
+    Serial.print(ins.rotX());
+    Serial.print("\t");
+    Serial.print(ins.rotY());
+    Serial.print("\t");
+    Serial.println(ins.rotZ());
+    Serial.print("spd :\t");
+    Serial.print(ins.posDX());
+    Serial.print("\t");
+    Serial.print(ins.posDY());
+    Serial.print("\t");
+    Serial.print(ins.posDZ());
+    Serial.print("\t");
+    Serial.print(ins.rotDX());
+    Serial.print("\t");
+    Serial.print(ins.rotDY());
+    Serial.print("\t");
+    Serial.println(ins.rotDZ());
+    Serial.print("grv :\t");
+    Serial.print(ins.grvX());
+    Serial.print("\t");
+    Serial.print(ins.grvY());
+    Serial.print("\t");
+    Serial.print(ins.grvZ());
+    Serial.print("\t(len: ");
+    Serial.print(sqrt(ins.grvX()*ins.grvX()+ins.grvY()*ins.grvY()+ins.grvZ()*ins.grvZ()));
+    Serial.println(")");
+    Serial.print("imu :\t");
+    Serial.print(imu.getAccelX_mss());
+    Serial.print("\t");
+    Serial.print(imu.getAccelY_mss());
+    Serial.print("\t");
+    Serial.print(imu.getAccelZ_mss());
+    Serial.print("\t");
+    Serial.print(imu.getGyroX_rads());
+    Serial.print("\t");
+    Serial.print(imu.getGyroY_rads());
+    Serial.print("\t");
+    Serial.println(imu.getGyroZ_rads());
+    Serial.print("img :\t");
+    Serial.print(imu.getAccelX_mss() - ins.grvX());
+    Serial.print("\t");
+    Serial.print(imu.getAccelY_mss() - ins.grvY());
+    Serial.print("\t");
+    Serial.println(imu.getAccelZ_mss() - ins.grvZ());
+  }
+
+  prevTime = nowTime;
+}

wuhyuo.sln

+
+Microsoft Visual Studio Solution File, Format Version 12.00
+# Visual Studio 15
+VisualStudioVersion = 15.0.28307.572
+MinimumVisualStudioVersion = 10.0.40219.1
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "wuhyuo", "wuhyuo\wuhyuo.csproj", "{990FED84-DE8A-4474-BAC7-082B64843B7D}"
+EndProject
+Global
+	GlobalSection(SolutionConfigurationPlatforms) = preSolution
+		Debug|Any CPU = Debug|Any CPU
+		Release|Any CPU = Release|Any CPU
+	EndGlobalSection
+	GlobalSection(ProjectConfigurationPlatforms) = postSolution
+		{990FED84-DE8A-4474-BAC7-082B64843B7D}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{990FED84-DE8A-4474-BAC7-082B64843B7D}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{990FED84-DE8A-4474-BAC7-082B64843B7D}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{990FED84-DE8A-4474-BAC7-082B64843B7D}.Release|Any CPU.Build.0 = Release|Any CPU
+	EndGlobalSection
+	GlobalSection(SolutionProperties) = preSolution
+		HideSolutionNode = FALSE
+	EndGlobalSection
+	GlobalSection(ExtensibilityGlobals) = postSolution
+		SolutionGuid = {3EA65EE4-96B4-43A7-B2F2-888D0952003B}
+	EndGlobalSection
+EndGlobal

wuhyuo/App.config

+<?xml version="1.0" encoding="utf-8" ?>
+<configuration>
+    <startup> 
+        <supportedRuntime version="v4.0" sku=".NETFramework,Version=v4.6.1" />
+    </startup>
+</configuration>
\ No newline at end of file

wuhyuo/App.xaml

+<Application x:Class="wuhyuo.App"
+             xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
+             xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
+             xmlns:local="clr-namespace:wuhyuo"
+             StartupUri="MainWindow.xaml">
+    <Application.Resources>
+         
+    </Application.Resources>
+</Application>

wuhyuo/App.xaml.cs

+using System;
+using System.Collections.Generic;
+using System.Configuration;
+using System.Data;
+using System.Linq;
+using System.Threading.Tasks;
+using System.Windows;
+
+namespace wuhyuo
+{
+    /// <summary>
+    /// App.xaml에 대한 상호 작용 논리
+    /// </summary>
+    public partial class App : Application
+    {
+    }
+}

wuhyuo/MainWindow.xaml

+<Window x:Class="wuhyuo.MainWindow"
+        xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
+        xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
+        xmlns:d="http://schemas.microsoft.com/expression/blend/2008"
+        xmlns:mc="http://schemas.openxmlformats.org/markup-compatibility/2006"
+        xmlns:local="clr-namespace:wuhyuo"
+        mc:Ignorable="d"
+        Title="MainWindow" Height="450" Width="800" Initialized="Window_Initialized" Closed="Window_Closed">
+	<Canvas HorizontalAlignment="Center" VerticalAlignment="Center" RenderTransform="1 0 0 -1 0 0">
+		<Rectangle x:Name="rect" Canvas.Left="0" Canvas.Top="0" Width="50" Height="50" Fill="#FF6BE8B5" RenderTransformOrigin="0.5,0.5" >
+			<Rectangle.RenderTransform>
+				<TransformGroup>
+					<RotateTransform x:Name="rectRotate" Angle="0"/>
+					<TranslateTransform X="-25" Y="-25"/>
+				</TransformGroup>
+			</Rectangle.RenderTransform>
+		</Rectangle>
+	</Canvas>
+</Window>

wuhyuo/MainWindow.xaml.cs

+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+using System.Windows;
+using System.Windows.Controls;
+using System.Windows.Data;
+using System.Windows.Documents;
+using System.Windows.Input;
+using System.Windows.Media;
+using System.Windows.Media.Imaging;
+using System.Windows.Navigation;
+using System.Windows.Shapes;
+using System.Threading;
+using System.IO.Ports;
+using System.Diagnostics;
+
+namespace wuhyuo
+{
+    /// <summary>
+    /// MainWindow.xaml에 대한 상호 작용 논리
+    /// </summary>
+    public partial class MainWindow : Window
+    {
+        public MainWindow()
+        {
+            InitializeComponent();
+        }
+
+        private void Window_Initialized(object sender, EventArgs e)
+        {
+            _thrd = new Thread(InputThread);
+            _thrd.Start();
+        }
+
+        private void Window_Closed(object sender, EventArgs e)
+        {
+            _thrd.Abort();
+            _thrd.Join();
+        }
+
+        private void InputThread()
+        {
+            try
+            {
+                using (var port = new SerialPort("COM6", 115200))
+                {
+                    port.Open();
+                    while (true)
+                    {
+                        try
+                        {
+                            string input = port.ReadLine();
+                            double[] values = input.Split(" \t\r\n".ToArray()).Where(x => x.Length > 0)
+                                .Select(x => Convert.ToDouble(x)).ToArray();
+                            if (values.Length != 6)
+                                continue;
+
+                            Dispatcher.Invoke(() =>
+                            {
+                                Debug.WriteLine(input);
+                                Canvas.SetLeft(rect, values[0] * 10);
+                                Canvas.SetRight(rect, values[1] * 10);
+                                rectRotate.Angle = values[5] * 180 / Math.PI;
+                            });
+                        }
+                        catch (FormatException) { }
+                        catch (TimeoutException) { }
+                    }
+                }
+            }
+            catch (InvalidOperationException)
+            {
+                Dispatcher.Invoke(() =>
+                {
+                    MessageBox.Show("arduino connection closed");
+                    Close();
+                });
+            }
+        }
+
+        private Thread _thrd;
+    }
+}

wuhyuo/Properties/AssemblyInfo.cs

+using System.Reflection;
+using System.Resources;
+using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
+using System.Windows;
+
+// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 
+// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면
+// 이러한 특성 값을 변경하세요.
+[assembly: AssemblyTitle("wuhyuo")]
+[assembly: AssemblyDescription("")]
+[assembly: AssemblyConfiguration("")]
+[assembly: AssemblyCompany("")]
+[assembly: AssemblyProduct("wuhyuo")]
+[assembly: AssemblyCopyright("Copyright ©  2019")]
+[assembly: AssemblyTrademark("")]
+[assembly: AssemblyCulture("")]
+
+// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 
+// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면
+// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요.
+[assembly: ComVisible(false)]
+
+//지역화 가능 응용 프로그램 빌드를 시작하려면 다음을 설정하세요.
+//.csproj 파일에서 <PropertyGroup> 내에 <UICulture>CultureYouAreCodingWith</UICulture>를
+//설정하십시오. 예를 들어 소스 파일에서 영어(미국)를
+//사용하는 경우 <UICulture>를 en-US로 설정합니다. 그런 다음 아래
+//NeutralResourceLanguage 특성의 주석 처리를 제거합니다. 아래 줄의 "en-US"를 업데이트하여
+//프로젝트 파일의 UICulture 설정과 일치시킵니다.
+
+//[assembly: NeutralResourcesLanguage("en-US", UltimateResourceFallbackLocation.Satellite)]
+
+
+[assembly: ThemeInfo(
+    ResourceDictionaryLocation.None, //테마별 리소스 사전의 위치
+                                     //(페이지 또는 응용 프로그램 리소스 사진에
+                                     // 리소스가 없는 경우에 사용됨)
+    ResourceDictionaryLocation.SourceAssembly //제네릭 리소스 사전의 위치
+                                              //(페이지 또는 응용 프로그램 리소스 사진에
+                                              // 리소스가 없는 경우에 사용됨)
+)]
+
+
+// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다.
+//
+//      주 버전
+//      부 버전 
+//      빌드 번호
+//      수정 버전
+//
+// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로
+// 지정되도록 할 수 있습니다.
+// [assembly: AssemblyVersion("1.0.*")]
+[assembly: AssemblyVersion("1.0.0.0")]
+[assembly: AssemblyFileVersion("1.0.0.0")]

wuhyuo/Properties/Resources.Designer.cs

+//------------------------------------------------------------------------------
+// <auto-generated>
+//     이 코드는 도구를 사용하여 생성되었습니다.
+//     런타임 버전:4.0.30319.42000
+//
+//     파일 내용을 변경하면 잘못된 동작이 발생할 수 있으며, 코드를 다시 생성하면
+//     이러한 변경 내용이 손실됩니다.
+// </auto-generated>
+//------------------------------------------------------------------------------
+
+namespace wuhyuo.Properties
+{
+
+
+    /// <summary>
+    ///   지역화된 문자열 등을 찾기 위한 강력한 형식의 리소스 클래스입니다.
+    /// </summary>
+    // 이 클래스는 ResGen 또는 Visual Studio와 같은 도구를 통해 StronglyTypedResourceBuilder
+    // 클래스에서 자동으로 생성되었습니다.
+    // 멤버를 추가하거나 제거하려면 .ResX 파일을 편집한 다음 /str 옵션을 사용하여
+    // ResGen을 다시 실행하거나 VS 프로젝트를 다시 빌드하십시오.
+    [global::System.CodeDom.Compiler.GeneratedCodeAttribute("System.Resources.Tools.StronglyTypedResourceBuilder", "4.0.0.0")]
+    [global::System.Diagnostics.DebuggerNonUserCodeAttribute()]
+    [global::System.Runtime.CompilerServices.CompilerGeneratedAttribute()]
+    internal class Resources
+    {
+
+        private static global::System.Resources.ResourceManager resourceMan;
+
+        private static global::System.Globalization.CultureInfo resourceCulture;
+
+        [global::System.Diagnostics.CodeAnalysis.SuppressMessageAttribute("Microsoft.Performance", "CA1811:AvoidUncalledPrivateCode")]
+        internal Resources()
+        {
+        }
+
+        /// <summary>
+        ///   이 클래스에서 사용하는 캐시된 ResourceManager 인스턴스를 반환합니다.
+        /// </summary>
+        [global::System.ComponentModel.EditorBrowsableAttribute(global::System.ComponentModel.EditorBrowsableState.Advanced)]
+        internal static global::System.Resources.ResourceManager ResourceManager {
+            get {
+                if ((resourceMan == null))
+                {
+                    global::System.Resources.ResourceManager temp = new global::System.Resources.ResourceManager("wuhyuo.Properties.Resources", typeof(Resources).Assembly);
+                    resourceMan = temp;
+                }
+                return resourceMan;
+            }
+        }
+
+        /// <summary>
+        ///   이 강력한 형식의 리소스 클래스를 사용하여 모든 리소스 조회에 대해 현재 스레드의 CurrentUICulture 속성을
+        ///   재정의합니다.
+        /// </summary>
+        [global::System.ComponentModel.EditorBrowsableAttribute(global::System.ComponentModel.EditorBrowsableState.Advanced)]
+        internal static global::System.Globalization.CultureInfo Culture {
+            get {
+                return resourceCulture;
+            }
+            set {
+                resourceCulture = value;
+            }
+        }
+    }
+}

wuhyuo/Properties/Resources.resx

+<?xml version="1.0" encoding="utf-8"?>
+<root>
+  <!-- 
+    Microsoft ResX Schema 
+    
+    Version 2.0
+    
+    The primary goals of this format is to allow a simple XML format 
+    that is mostly human readable. The generation and parsing of the 
+    various data types are done through the TypeConverter classes 
+    associated with the data types.
+    
+    Example:
+    
+    ... ado.net/XML headers & schema ...
+    <resheader name="resmimetype">text/microsoft-resx</resheader>
+    <resheader name="version">2.0</resheader>
+    <resheader name="reader">System.Resources.ResXResourceReader, System.Windows.Forms, ...</resheader>
+    <resheader name="writer">System.Resources.ResXResourceWriter, System.Windows.Forms, ...</resheader>
+    <data name="Name1"><value>this is my long string</value><comment>this is a comment</comment></data>
+    <data name="Color1" type="System.Drawing.Color, System.Drawing">Blue</data>
+    <data name="Bitmap1" mimetype="application/x-microsoft.net.object.binary.base64">
+        <value>[base64 mime encoded serialized .NET Framework object]</value>
+    </data>
+    <data name="Icon1" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">
+        <value>[base64 mime encoded string representing a byte array form of the .NET Framework object]</value>
+        <comment>This is a comment</comment>
+    </data>
+                
+    There are any number of "resheader" rows that contain simple 
+    name/value pairs.
+    
+    Each data row contains a name, and value. The row also contains a 
+    type or mimetype. Type corresponds to a .NET class that support 
+    text/value conversion through the TypeConverter architecture. 
+    Classes that don't support this are serialized and stored with the 
+    mimetype set.
+    
+    The mimetype is used for serialized objects, and tells the 
+    ResXResourceReader how to depersist the object. This is currently not 
+    extensible. For a given mimetype the value must be set accordingly:
+    
+    Note - application/x-microsoft.net.object.binary.base64 is the format 
+    that the ResXResourceWriter will generate, however the reader can 
+    read any of the formats listed below.
+    
+    mimetype: application/x-microsoft.net.object.binary.base64
+    value   : The object must be serialized with 
+            : System.Serialization.Formatters.Binary.BinaryFormatter
+            : and then encoded with base64 encoding.
+    
+    mimetype: application/x-microsoft.net.object.soap.base64
+    value   : The object must be serialized with 
+            : System.Runtime.Serialization.Formatters.Soap.SoapFormatter
+            : and then encoded with base64 encoding.
+
+    mimetype: application/x-microsoft.net.object.bytearray.base64
+    value   : The object must be serialized into a byte array 
+            : using a System.ComponentModel.TypeConverter
+            : and then encoded with base64 encoding.
+    -->
+  <xsd:schema id="root" xmlns="" xmlns:xsd="http://www.w3.org/2001/XMLSchema" xmlns:msdata="urn:schemas-microsoft-com:xml-msdata">
+    <xsd:element name="root" msdata:IsDataSet="true">
+      <xsd:complexType>
+        <xsd:choice maxOccurs="unbounded">
+          <xsd:element name="metadata">
+            <xsd:complexType>
+              <xsd:sequence>
+                <xsd:element name="value" type="xsd:string" minOccurs="0" />
+              </xsd:sequence>
+              <xsd:attribute name="name" type="xsd:string" />
+              <xsd:attribute name="type" type="xsd:string" />
+              <xsd:attribute name="mimetype" type="xsd:string" />
+            </xsd:complexType>
+          </xsd:element>
+          <xsd:element name="assembly">
+            <xsd:complexType>
+              <xsd:attribute name="alias" type="xsd:string" />
+              <xsd:attribute name="name" type="xsd:string" />
+            </xsd:complexType>
+          </xsd:element>
+          <xsd:element name="data">
+            <xsd:complexType>
+              <xsd:sequence>
+                <xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
+                <xsd:element name="comment" type="xsd:string" minOccurs="0" msdata:Ordinal="2" />
+              </xsd:sequence>
+              <xsd:attribute name="name" type="xsd:string" msdata:Ordinal="1" />
+              <xsd:attribute name="type" type="xsd:string" msdata:Ordinal="3" />
+              <xsd:attribute name="mimetype" type="xsd:string" msdata:Ordinal="4" />
+            </xsd:complexType>
+          </xsd:element>
+          <xsd:element name="resheader">
+            <xsd:complexType>
+              <xsd:sequence>
+                <xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
+              </xsd:sequence>
+              <xsd:attribute name="name" type="xsd:string" use="required" />
+            </xsd:complexType>
+          </xsd:element>
+        </xsd:choice>
+      </xsd:complexType>
+    </xsd:element>
+  </xsd:schema>
+  <resheader name="resmimetype">
+    <value>text/microsoft-resx</value>
+  </resheader>
+  <resheader name="version">
+    <value>2.0</value>
+  </resheader>
+  <resheader name="reader">
+    <value>System.Resources.ResXResourceReader, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
+  </resheader>
+  <resheader name="writer">
+    <value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
+  </resheader>
+</root>
\ No newline at end of file

wuhyuo/Properties/Settings.Designer.cs

+//------------------------------------------------------------------------------
+// <auto-generated>
+//     This code was generated by a tool.
+//     Runtime Version:4.0.30319.42000
+//
+//     Changes to this file may cause incorrect behavior and will be lost if
+//     the code is regenerated.
+// </auto-generated>
+//------------------------------------------------------------------------------
+
+namespace wuhyuo.Properties
+{
+
+
+    [global::System.Runtime.CompilerServices.CompilerGeneratedAttribute()]
+    [global::System.CodeDom.Compiler.GeneratedCodeAttribute("Microsoft.VisualStudio.Editors.SettingsDesigner.SettingsSingleFileGenerator", "11.0.0.0")]
+    internal sealed partial class Settings : global::System.Configuration.ApplicationSettingsBase
+    {
+
+        private static Settings defaultInstance = ((Settings)(global::System.Configuration.ApplicationSettingsBase.Synchronized(new Settings())));
+
+        public static Settings Default {
+            get {
+                return defaultInstance;
+            }
+        }
+    }
+}

wuhyuo/Properties/Settings.settings

+<?xml version='1.0' encoding='utf-8'?>
+<SettingsFile xmlns="uri:settings" CurrentProfile="(Default)">
+  <Profiles>
+    <Profile Name="(Default)" />
+  </Profiles>
+  <Settings />
+</SettingsFile>
\ No newline at end of file

wuhyuo/wuhyuo.csproj

+<?xml version="1.0" encoding="utf-8"?>
+<Project ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')" />
+  <PropertyGroup>
+    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
+    <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
+    <ProjectGuid>{990FED84-DE8A-4474-BAC7-082B64843B7D}</ProjectGuid>
+    <OutputType>WinExe</OutputType>
+    <RootNamespace>wuhyuo</RootNamespace>
+    <AssemblyName>wuhyuo</AssemblyName>
+    <TargetFrameworkVersion>v4.6.1</TargetFrameworkVersion>
+    <FileAlignment>512</FileAlignment>
+    <ProjectTypeGuids>{60dc8134-eba5-43b8-bcc9-bb4bc16c2548};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}</ProjectTypeGuids>
+    <WarningLevel>4</WarningLevel>
+    <AutoGenerateBindingRedirects>true</AutoGenerateBindingRedirects>
+    <Deterministic>true</Deterministic>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
+    <PlatformTarget>AnyCPU</PlatformTarget>
+    <DebugSymbols>true</DebugSymbols>
+    <DebugType>full</DebugType>
+    <Optimize>false</Optimize>
+    <OutputPath>bin\Debug\</OutputPath>
+    <DefineConstants>DEBUG;TRACE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
+    <PlatformTarget>AnyCPU</PlatformTarget>
+    <DebugType>pdbonly</DebugType>
+    <Optimize>true</Optimize>
+    <OutputPath>bin\Release\</OutputPath>
+    <DefineConstants>TRACE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+  </PropertyGroup>
+  <ItemGroup>
+    <Reference Include="System" />
+    <Reference Include="System.Data" />
+    <Reference Include="System.Xml" />
+    <Reference Include="Microsoft.CSharp" />
+    <Reference Include="System.Core" />
+    <Reference Include="System.Xml.Linq" />
+    <Reference Include="System.Data.DataSetExtensions" />
+    <Reference Include="System.Net.Http" />
+    <Reference Include="System.Xaml">
+      <RequiredTargetFramework>4.0</RequiredTargetFramework>
+    </Reference>
+    <Reference Include="WindowsBase" />
+    <Reference Include="PresentationCore" />
+    <Reference Include="PresentationFramework" />
+  </ItemGroup>
+  <ItemGroup>
+    <ApplicationDefinition Include="App.xaml">
+      <Generator>MSBuild:Compile</Generator>
+      <SubType>Designer</SubType>
+    </ApplicationDefinition>
+    <Page Include="MainWindow.xaml">
+      <Generator>MSBuild:Compile</Generator>
+      <SubType>Designer</SubType>
+    </Page>
+    <Compile Include="App.xaml.cs">
+      <DependentUpon>App.xaml</DependentUpon>
+      <SubType>Code</SubType>
+    </Compile>
+    <Compile Include="MainWindow.xaml.cs">
+      <DependentUpon>MainWindow.xaml</DependentUpon>
+      <SubType>Code</SubType>
+    </Compile>
+  </ItemGroup>
+  <ItemGroup>
+    <Compile Include="Properties\AssemblyInfo.cs">
+      <SubType>Code</SubType>
+    </Compile>
+    <Compile Include="Properties\Resources.Designer.cs">
+      <AutoGen>True</AutoGen>
+      <DesignTime>True</DesignTime>
+      <DependentUpon>Resources.resx</DependentUpon>
+    </Compile>
+    <Compile Include="Properties\Settings.Designer.cs">
+      <AutoGen>True</AutoGen>
+      <DependentUpon>Settings.settings</DependentUpon>
+      <DesignTimeSharedInput>True</DesignTimeSharedInput>
+    </Compile>
+    <EmbeddedResource Include="Properties\Resources.resx">
+      <Generator>ResXFileCodeGenerator</Generator>
+      <LastGenOutput>Resources.Designer.cs</LastGenOutput>
+    </EmbeddedResource>
+    <None Include="Properties\Settings.settings">
+      <Generator>SettingsSingleFileGenerator</Generator>
+      <LastGenOutput>Settings.Designer.cs</LastGenOutput>
+    </None>
+  </ItemGroup>
+  <ItemGroup>
+    <None Include="App.config" />
+  </ItemGroup>
+  <Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
+</Project>
\ No newline at end of file