/// <summary> /// Reads raw sensor data from the device then updates <see cref="AxisOffset"/> property. /// </summary> public virtual Mpu9250OffsetReading ReadOffset() { Mpu9250OffsetReading axisOffset = new Mpu9250OffsetReading(); // Read the data out registers var accelBytes = Hardware.WriteReadBytes((byte)Mpu9250Register.OffsetXAHigh, 6); axisOffset.AccelXAxisOffset = (short)(accelBytes[1] << 8 | accelBytes[0]); axisOffset.AccelYAxisOffset = (short)(accelBytes[3] << 8 | accelBytes[2]); axisOffset.AccelZAxisOffset = (short)(accelBytes[5] << 8 | accelBytes[4]); // Read the data out registers var gyroBytes = Hardware.WriteReadBytes((byte)Mpu9250Register.OffsetXGHigh, 6); axisOffset.GyroXAxisOffset = (short)(gyroBytes[1] << 8 | gyroBytes[0]); axisOffset.GyroYAxisOffset = (short)(gyroBytes[3] << 8 | gyroBytes[2]); axisOffset.GyroZAxisOffset = (short)(gyroBytes[5] << 8 | gyroBytes[4]); // Read the data out registers var magBytes = ReadMagSensitivity(); axisOffset.MagXAxisSensitivity = magBytes[0]; axisOffset.MagYAxisSensitivity = magBytes[1]; axisOffset.MagZAxisSensitivity = magBytes[2]; _axisOffset = axisOffset; // Return results return(axisOffset); }
public Mpu9250Device(SpiDevice device) { // Validate if (device == null) { throw new ArgumentNullException(nameof(device)); } // Initialize hardware Hardware = device; // Reset the device to default setting this.Reset(); // Set clock source Hardware.WriteJoinByte((byte)Mpu9250Register.PowerManagment1, 0x01); // Set I2C master mode Hardware.WriteJoinByte((byte)Mpu9250Register.UserControl, 0x20); // Set I2C configureation multi-master IIC 400KHz. Hardware.WriteJoinByte((byte)Mpu9250Register.I2cMasterControl, 0x0D); // Set temprature bandwidth to 188Hz and gyroscope bandwidth to 184Hz. Hardware.WriteJoinByte((byte)Mpu9250Register.Config, 0x00); // Verify the device is connected, accessable and ready for use. if (ChipId == 0x71 & Ak8963ChipId == 0x48) { IsConnected = true; } // Initialize sensor reading SensorReading = new Mpu9250SensorReading(); // Initialize sensor axis offset data AxisOffset = new Mpu9250OffsetReading() { MagXAxisSensitivity = 1, MagYAxisSensitivity = 1, MagZAxisSensitivity = 1 }; }