MPU9250.cpp

/*
 *
 * https://github.com/bolderflight/MPU9250
MPU9250.cpp
Brian R Taylor
brian.taylor@bolderflight.com

Copyright (c) 2017 Bolder Flight Systems
changed by B. Seeger to be usable at stm32 with HAl 2019

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 "MPU9250.h"

/* MPU9250 object, input the SPI bus and chip select pin */
MPU9250::MPU9250(GPIO_TypeDef* SPICSTypeDef, uint16_t SPICSPin,SPI_HandleTypeDef* MPU9250spi,uint32_t BaseID){
    _SPICSTypeDef=SPICSTypeDef;
    _SPICSPin=SPICSPin;
    _MPU9250spi=MPU9250spi;
	_BaseID=BaseID;
	_SetingsID=0;
	_ID=_BaseID+(uint32_t)_SetingsID;
}

int MPU9250::setBaseID(uint32_t BaseID)
{
	_BaseID=BaseID;
	_SetingsID=0;
	_ID=_BaseID+(uint32_t)_SetingsID;
	return 0;
}
/* starts communication with the MPU-9250 */
int MPU9250::begin(){
	// spi interface is maybe not ready if the class instance is created but on begin it musst be ready so we are getting the default speed now from the interface
	// using SPI for communication
	// use low speed SPI for register setting
	_useSPIHS = false;
	  // select clock source to gyro
	  if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0){
	    return -1;
	  }

	  //read selftest data
	  readRegisters(ACC_ST_X, 3, _AccST);
	  readRegisters(GYRO_ST_X, 3, _GyroST);

	  // 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
	  HAL_Delay(200);
	  // reset the AK8963
	  writeAK8963Register(AK8963_CNTL2,AK8963_RESET);

	  HAL_Delay(200);
	  // 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 4G
	  _accelRange = ACCEL_RANGE_4G;
	  // 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;
	  }
	  HAL_Delay(200); // long wait between AK8963 mode changes
	  // set AK8963 to FUSE ROM access
	  if(writeAK8963Register(AK8963_CNTL1,AK8963_FUSE_ROM) < 0){
	    //return -16;
	  }
	  HAL_Delay(200); // 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;
	  }
	  HAL_Delay(200); // 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;
	  }
	 HAL_Delay(200); // long wait between AK8963 mode changes
	  // select clock source to gyro
	  if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0){
	    //return -19;
	  }
	  HAL_Delay(200);
	  // 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;
	  //}
	  //Dummy read to active High speed SPI
	  readSensor();
	  readSensor();
	  // successful init, return 1
	  return 1;
	}

/* sets the accelerometer full scale range to values other than default */
int MPU9250::setAccelRange(AccelRange range) {
	_SetingsID++;
	_ID=_BaseID+(uint32_t)_SetingsID;
  // 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) {
	_SetingsID++;
	_ID=_BaseID+(uint32_t)_SetingsID;
  // 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) {
	_SetingsID++;
	_ID=_BaseID+(uint32_t)_SetingsID;
  // use low speed SPI for register setting
  _useSPIHS = false;
  switch(bandwidth) {
  case DLPF_BANDWIDTH_250HZ: {
    if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_250) < 0){ // setting accel bandwidth to 184Hz
      return -1;
    }
    if(writeRegister(CONFIG,GYRO_DLPF_250) < 0){ // setting gyro bandwidth to 184Hz
      return -2;
    }
    break;
  }
    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;
	    }
	    HAL_Delay(250); // 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;
	    }
	    HAL_Delay(250); // 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;
	    }
	    HAL_Delay(250); // 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;
	    }
	    HAL_Delay(250); // 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
  HAL_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() {
	_SetingsID++;
	_ID=_BaseID+(uint32_t)_SetingsID;
  // 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);
    HAL_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() {
	_SetingsID++;
	_ID=_BaseID+(uint32_t)_SetingsID;
  // 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);
    HAL_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() {
	_SetingsID++;
	_ID=_BaseID+(uint32_t)_SetingsID;
  // 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++;
    }
    HAL_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;
}
/* Sets the Acc selftest registers bytemask 0x00000xyz 1=selftest active 0=normal mesurment */
void MPU9250::setAccSelfTest(uint8_t SelftestStatus){
	// mutex not nesseary since isr can interrupt this routine but not the write regisrer routine since SPi will beblocked
	//bytemask 0x00000xyz 1=selftest active 0=normal mesurment
	_AccSeftestStatus=0x07 & SelftestStatus;
	uint8_t accStatusToSet=_AccSeftestStatus<<5;
	uint8_t configOLD=0;
	readRegisters(ACCEL_CONFIG,1,&configOLD);
	uint8_t configNew=(configOLD&0x1F) |accStatusToSet;
	writeRegister(ACCEL_CONFIG,configNew);
}
/* Sets the Gyro selftest registers bytemask 0x00000xyz 1=selftest active 0=normal mesurment */
void MPU9250::setGyroSelfTest(uint8_t SelftestStatus){
	//bytemask 0x00000xyz 1=selftest active 0=normal mesurment
	// mutex not nesseary since isr can interrupt this routine but not the write regisrer routine since SPi will beblocked
	_GyroSeftestStatus=0x07 & SelftestStatus;
	uint8_t gyroStatusToSet=_GyroSeftestStatus<<5;
	uint8_t configOLD=0;
	readRegisters(GYRO_CONFIG,1,&configOLD);
	uint8_t configNew=(configOLD&0x1F) |gyroStatusToSet;
	writeRegister(GYRO_CONFIG,configNew);
}
/* 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(_SPIHSBOUDRATEPRESCALERSLOW!=_MPU9250spi->Init.BaudRatePrescaler){
    	_MPU9250spi->Init.BaudRatePrescaler = _SPIHSBOUDRATEPRESCALERSLOW;
    	HAL_SPI_Init(_MPU9250spi);
    	_useSPILSOLD=true;
    }
    if(_useSPIHS){
    	if(_SPIHSBOUDRATEPRESCALERFAST!=_MPU9250spi->Init.BaudRatePrescaler){
    	_MPU9250spi->Init.BaudRatePrescaler =_SPIHSBOUDRATEPRESCALERFAST;
    	HAL_StatusTypeDef SpiRetVal= HAL_ERROR;
    	while(SpiRetVal== HAL_ERROR){
    		SpiRetVal=HAL_SPI_Init(_MPU9250spi);
    	}
    	_useSPILSOLD=false;

    }
    }
  	uint8_t buffer[2] = {subAddress, data };
  	HAL_GPIO_WritePin(_SPICSTypeDef, _SPICSPin, GPIO_PIN_RESET);
  	HAL_SPI_Transmit(_MPU9250spi, buffer, 2, SPI_TIMEOUT);
  	HAL_GPIO_WritePin(_SPICSTypeDef, _SPICSPin, GPIO_PIN_SET);
  /* 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){
    // begin the transaction
    if(_SPIHSBOUDRATEPRESCALERSLOW!=_MPU9250spi->Init.BaudRatePrescaler){
    	_MPU9250spi->Init.BaudRatePrescaler = _SPIHSBOUDRATEPRESCALERSLOW;
    	HAL_SPI_Init(_MPU9250spi);
    	_useSPILSOLD=true;
    }
    if(_useSPIHS){
    	if(_SPIHSBOUDRATEPRESCALERFAST!=_MPU9250spi->Init.BaudRatePrescaler){
    	_MPU9250spi->Init.BaudRatePrescaler =_SPIHSBOUDRATEPRESCALERFAST;
    	HAL_StatusTypeDef SpiRetVal= HAL_ERROR;
    	while(SpiRetVal== HAL_ERROR){
    		SpiRetVal=HAL_SPI_Init(_MPU9250spi);
    	}
    	_useSPILSOLD=false;

    }
    }
  	int retVal=0;
  	uint8_t tx[count+1]={0};
  	uint8_t rx[count+1]={0};
  	tx[0] = {SPI_READ |subAddress};
  	HAL_GPIO_WritePin(_SPICSTypeDef, _SPICSPin, GPIO_PIN_RESET);
  	if(HAL_SPI_TransmitReceive(_MPU9250spi,tx, rx, count+1, SPI_TIMEOUT)==HAL_OK)
  	{
  		retVal=1;
  	}
  	HAL_GPIO_WritePin(_SPICSTypeDef, _SPICSPin, GPIO_PIN_SET);
  	memcpy(dest, &rx[1], count);
  return retVal;
}

/* 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;
  }
	HAL_Delay(50);
	// 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;
  }
	HAL_Delay(50); // 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];
}

int MPU9250::getData(DataMessage * Message,uint64_t RawTimeStamp,uint32_t CaptureCount){
	memcpy(Message,&empty_DataMessage,sizeof(DataMessage));//Copy default values into array
	int result=0;
	Message->id=_ID;
	Message->unix_time=0XFFFFFFFF;
	Message->time_uncertainty=(uint32_t)((RawTimeStamp & 0xFFFFFFFF00000000) >> 32);//high word
	Message->unix_time_nsecs=(uint32_t)(RawTimeStamp & 0x00000000FFFFFFFF);// low word
	Message->sample_number=CaptureCount;
	result=MPU9250::readSensor();
	/*
	  _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;
	  */
	Message->Data_01=_ax;
	Message->has_Data_02=true;
	Message->Data_02=_ay;
	Message->has_Data_03=true;
	Message->Data_03=_az;
	Message->has_Data_04=true;
	Message->Data_04=_gx;
	Message->has_Data_05=true;
	Message->Data_05=_gy;
	Message->has_Data_06=true;
	Message->Data_06=_gz;
	Message->has_Data_07=true;
	Message->Data_07=_hx;
	Message->has_Data_08=true;
	Message->Data_08=_hy;
	Message->has_Data_09=true;
	Message->Data_09=_hz;
	Message->has_Data_10=true;
	Message->Data_10=_t;
	return result;
}

int MPU9250::getDescription(DescriptionMessage * Message,DescriptionMessage_DESCRIPTION_TYPE DESCRIPTION_TYPE){
	memcpy(Message,&empty_DescriptionMessage,sizeof(DescriptionMessage));//Copy default values into array
	int retVal=0;
	strncpy(Message->Sensor_name,"MPU 9250\0",sizeof(Message->Sensor_name));
	Message->id=_ID;
	Message->Description_Type=DESCRIPTION_TYPE;
	if(DESCRIPTION_TYPE==DescriptionMessage_DESCRIPTION_TYPE_PHYSICAL_QUANTITY)
	{
		Message->Description_Type=DescriptionMessage_DESCRIPTION_TYPE_PHYSICAL_QUANTITY;
		Message->has_str_Data_01=true;
		Message->has_str_Data_02=true;
		Message->has_str_Data_03=true;
		Message->has_str_Data_04=true;
		Message->has_str_Data_05=true;
		Message->has_str_Data_06=true;
		Message->has_str_Data_07=true;
		Message->has_str_Data_08=true;
		Message->has_str_Data_09=true;
		Message->has_str_Data_10=true;
		strncpy(Message->str_Data_01,"X Acceleration\0",sizeof(Message->str_Data_01));
		strncpy(Message->str_Data_02,"Y Acceleration\0",sizeof(Message->str_Data_02));
		strncpy(Message->str_Data_03,"Z Acceleration\0",sizeof(Message->str_Data_03));
		strncpy(Message->str_Data_04,"X Angular velocity\0",sizeof(Message->str_Data_04));
		strncpy(Message->str_Data_05,"Y Angular velocity\0",sizeof(Message->str_Data_05));
		strncpy(Message->str_Data_06,"Z Angular velocity\0",sizeof(Message->str_Data_06));
		strncpy(Message->str_Data_07,"X Magnetic flux density\0",sizeof(Message->str_Data_07));
		strncpy(Message->str_Data_08,"Y Magnetic flux density\0",sizeof(Message->str_Data_08));
		strncpy(Message->str_Data_09,"Z Magnetic flux density\0",sizeof(Message->str_Data_09));
		strncpy(Message->str_Data_10,"Temperature\0",sizeof(Message->str_Data_10));
	}
	if(DESCRIPTION_TYPE==DescriptionMessage_DESCRIPTION_TYPE_UNIT)
	{
		Message->Description_Type=DescriptionMessage_DESCRIPTION_TYPE_UNIT;
		Message->has_str_Data_01=true;
		Message->has_str_Data_02=true;
		Message->has_str_Data_03=true;
		Message->has_str_Data_04=true;
		Message->has_str_Data_05=true;
		Message->has_str_Data_06=true;
		Message->has_str_Data_07=true;
		Message->has_str_Data_08=true;
		Message->has_str_Data_09=true;
		Message->has_str_Data_10=true;
		strncpy(Message->str_Data_01,"\\metre\\second\\tothe{-2}\0",sizeof(Message->str_Data_01));
		strncpy(Message->str_Data_02,"\\metre\\second\\tothe{-2}\0",sizeof(Message->str_Data_02));
		strncpy(Message->str_Data_03,"\\metre\\second\\tothe{-2}\0",sizeof(Message->str_Data_03));
		strncpy(Message->str_Data_04,"\\radian\\second\\tothe{-1}\0",sizeof(Message->str_Data_04));
		strncpy(Message->str_Data_05,"\\radian\\second\\tothe{-1}\0",sizeof(Message->str_Data_05));
		strncpy(Message->str_Data_06,"\\radian\\second\\tothe{-1}\0",sizeof(Message->str_Data_06));
		strncpy(Message->str_Data_07,"\\micro\\tesla\0",sizeof(Message->str_Data_07));
		strncpy(Message->str_Data_08,"\\micro\\tesla\0",sizeof(Message->str_Data_08));
		strncpy(Message->str_Data_09,"\\micro\\tesla\0",sizeof(Message->str_Data_09));
		strncpy(Message->str_Data_10,"\\degreecelsius\0",sizeof(Message->str_Data_10));
	}
	if(DESCRIPTION_TYPE==DescriptionMessage_DESCRIPTION_TYPE_RESOLUTION)
	{
		Message->Description_Type=DescriptionMessage_DESCRIPTION_TYPE_RESOLUTION;
		Message->has_f_Data_01=true;
		Message->has_f_Data_02=true;
		Message->has_f_Data_03=true;
		Message->has_f_Data_04=true;
		Message->has_f_Data_05=true;
		Message->has_f_Data_06=true;
		Message->has_f_Data_07=true;
		Message->has_f_Data_08=true;
		Message->has_f_Data_09=true;
		Message->has_f_Data_10=true;
		Message->f_Data_01=65536;
		Message->f_Data_02=65536;
		Message->f_Data_03=65536;
		Message->f_Data_04=65536;
		Message->f_Data_05=65536;
		Message->f_Data_06=65536;
		Message->f_Data_07=65520;
		Message->f_Data_08=65520;
		Message->f_Data_09=65520;
		Message->f_Data_10=65536;
	}
	//TODO add min and max scale values as calls member vars so they have not to be calculated all the time
	if(DESCRIPTION_TYPE==DescriptionMessage_DESCRIPTION_TYPE_MIN_SCALE)
	{
		float accMIN=(((float)(-32768) * _accelScale) - _axb)*_axs;
		float gyrMIN=((float)(-32768) * _gyroScale) - _gxb;
		float magMIN=(((float)(-32760) * _magScaleX) - _hxb)*_hxs;
		float tempMIN= ((((float) -32768) - _tempOffset)/_tempScale) + _tempOffset;;
		Message->Description_Type=DescriptionMessage_DESCRIPTION_TYPE_MIN_SCALE;
		Message->has_f_Data_01=true;
		Message->has_f_Data_02=true;
		Message->has_f_Data_03=true;
		Message->has_f_Data_04=true;
		Message->has_f_Data_05=true;
		Message->has_f_Data_06=true;
		Message->has_f_Data_07=true;
		Message->has_f_Data_08=true;
		Message->has_f_Data_09=true;
		Message->has_f_Data_10=true;
		Message->f_Data_01=accMIN;
		Message->f_Data_02=accMIN;
		Message->f_Data_03=accMIN;
		Message->f_Data_04=gyrMIN;
		Message->f_Data_05=gyrMIN;
		Message->f_Data_06=gyrMIN;
		Message->f_Data_07=magMIN;
		Message->f_Data_08=magMIN;
		Message->f_Data_09=magMIN;
		Message->f_Data_10=tempMIN;
	}
	if(DESCRIPTION_TYPE==DescriptionMessage_DESCRIPTION_TYPE_MAX_SCALE)
	{
		float accMAX=(((float)(32767) * _accelScale) - _axb)*_axs;
		float gyrMAX=((float)(32767) * _gyroScale) - _gxb;
		float magMAX=(((float)(32760) * _magScaleX) - _hxb)*_hxs;
		float tempMAX= ((((float) 32767) - _tempOffset)/_tempScale) + _tempOffset;;
		Message->Description_Type=DescriptionMessage_DESCRIPTION_TYPE_MAX_SCALE;
		Message->has_f_Data_01=true;
		Message->has_f_Data_02=true;
		Message->has_f_Data_03=true;
		Message->has_f_Data_04=true;
		Message->has_f_Data_05=true;
		Message->has_f_Data_06=true;
		Message->has_f_Data_07=true;
		Message->has_f_Data_08=true;
		Message->has_f_Data_09=true;
		Message->has_f_Data_10=true;
		Message->f_Data_01=accMAX;
		Message->f_Data_02=accMAX;
		Message->f_Data_03=accMAX;
		Message->f_Data_04=gyrMAX;
		Message->f_Data_05=gyrMAX;
		Message->f_Data_06=gyrMAX;
		Message->f_Data_07=magMAX;
		Message->f_Data_08=magMAX;
		Message->f_Data_09=magMAX;
		Message->f_Data_10=tempMAX;
	}
	return retVal;
}