// Added to \libraries\MPU9250\MPU9250.cpp
int MPU9250::readSensorPMD(float* ax, float* ay, float* az, float* gx, float* gy, float* gz, float* hx, float* hy, float* hz, bool* MagD)
{
_useSPIHS = false; // use the high speed SPI for data readout
bool newMagData = 0;
uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
uint8_t dest[1];
readAK8963Registers(AK8963_ST1,1,dest);
newMagData = (dest[0] & 0x01);
readAK8963Registers(AK8963_HXL,7,rawData);
//readRegisters(EXT_SENS_DATA_00,7,&_buffer[0]);
readAK8963Registers(AK8963_ST2,1, dest); // End data read by reading ST2 register
if(newMagData == 1 && rawData[6] == 0x10) { // wait for magnetometer data ready bit to be set
_hxcounts = ((int16_t)rawData[1] << 8) | rawData[0];
_hycounts = ((int16_t)rawData[3] << 8) | rawData[2];
_hzcounts = ((int16_t)rawData[5] << 8) | rawData[4];
*MagD = true;
} else {
*MagD = false;
//_hxcounts = 0;
//_hycounts = 0;
//_hzcounts = 0;
}
// grab the data from the MPU9250
if (readRegisters(ACCEL_OUT, 14, _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];
// 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;
return 1;
}