// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
internal void CalibrateMpu9150(out double[] dest1, out double[] dest2) //This seems to work
{
dest1 = new double[3];
dest2 = new double[3];
byte[] data = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
// data array to hold accelerometer and gyro x, y, z, data
ushort ii, packetCount, fifoCount;
// reset device, reset all registers, clear gyro and accelerometer bias registers
_mpu9150.Write(PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
Task.Delay(200).Wait();
// get stable time source
// Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
_mpu9150.Write(PWR_MGMT_1, 0x01);
_mpu9150.Write(PWR_MGMT_2, 0x00);
Task.Delay(200).Wait();
// Configure device for bias calculation
_mpu9150.Write(INT_ENABLE, 0x00); // Disable all interrupts
_mpu9150.Write(FIFO_EN, 0x00); // Disable FIFO
_mpu9150.Write(PWR_MGMT_1, 0x00); // Turn on internal clock source
_mpu9150.Write(I2C_MST_CTRL, 0x00); // Disable I2C master
_mpu9150.Write(USER_CTRL, 0x00); // Disable FIFO and I2C master modes
_mpu9150.Write(USER_CTRL, 0x0C); // Reset FIFO and DMP
Task.Delay(50).Wait();
// Configure MPU9150 gyro and accelerometer for bias calculation
_mpu9150.Write(CONFIG, 0x01); // Set low-pass filter to 188 Hz
_mpu9150.Write(SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz
_mpu9150.Write(GYRO_CONFIG, 0x00);
// Set gyro full-scale to 250 degrees per second, maximum sensitivity
_mpu9150.Write(ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
ushort gyrosensitivity = 131; // = 131 LSB/degrees/sec
ushort accelsensitivity = 16384; // = 16384 LSB/g
// Configure FIFO to capture accelerometer and gyro data for bias calculation
_mpu9150.Write(USER_CTRL, 0x40); // Enable FIFO
_mpu9150.Write(FIFO_EN, 0x78);
// Enable gyro and accelerometer sensors for FIFO (max size 1024 bytes in MPU9150)
Task.Delay(200).Wait(); //Was 80?
// At end of sample accumulation, turn off FIFO sensor read
_mpu9150.Write(FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO
_mpu9150.Read(2, FIFO_COUNTH, out data); // read FIFO sample count
fifoCount = (ushort)((data[0] << 8) | data[1]);
packetCount = (ushort)(fifoCount / 12); // How many sets of full gyro and accelerometer data for averaging
for (ii = 0; ii < packetCount; ii++)
{
ushort[] accelTemp = { 0, 0, 0 }, gyroTemp = { 0, 0, 0 };
_mpu9150.Read(12, FIFO_R_W, out data); // read data for averaging
accelTemp[0] = (ushort)((data[0] << 8) | data[1]);
// Form unsigned 16-bit integer for each sample in FIFO
accelTemp[1] = (ushort)((data[2] << 8) | data[3]);
accelTemp[2] = (ushort)((data[4] << 8) | data[5]);
gyroTemp[0] = (ushort)((data[6] << 8) | data[7]);
gyroTemp[1] = (ushort)((data[8] << 8) | data[9]);
gyroTemp[2] = (ushort)((data[10] << 8) | data[11]);
accelBias[0] += accelTemp[0];
// Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
accelBias[1] += accelTemp[1];
accelBias[2] += accelTemp[2];
gyroBias[0] += gyroTemp[0];
gyroBias[1] += gyroTemp[1];
gyroBias[2] += gyroTemp[2];
}
accelBias[0] /= packetCount; // Normalize sums to get average count biases
accelBias[1] /= packetCount;
accelBias[2] /= packetCount;
gyroBias[0] /= packetCount;
gyroBias[1] /= packetCount;
gyroBias[2] /= packetCount;
if (accelBias[2] > 0L)
{
accelBias[2] -= accelsensitivity; // Remove gravity from the z-axis accelerometer bias calculation
}
else
{
accelBias[2] += accelsensitivity;
}
//There was a "-" in front of the gyro_bias in this section?
// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
data[0] = (byte)((gyroBias[0] / 4 >> 8) & 0xFF);
// Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
data[1] = (byte)((gyroBias[0] / 4) & 0xFF);
// Biases are additive, so change sign on calculated average gyro biases
data[2] = (byte)((gyroBias[1] / 4 >> 8) & 0xFF);
data[3] = (byte)((gyroBias[1] / 4) & 0xFF);
data[4] = (byte)((gyroBias[2] / 4 >> 8) & 0xFF);
data[5] = (byte)((gyroBias[2] / 4) & 0xFF);
// Push gyro biases to hardware registers
_mpu9150.Write(XG_OFFS_USRH, data[0]);
_mpu9150.Write(XG_OFFS_USRL, data[1]);
_mpu9150.Write(YG_OFFS_USRH, data[2]);
_mpu9150.Write(YG_OFFS_USRL, data[3]);
_mpu9150.Write(ZG_OFFS_USRH, data[4]);
_mpu9150.Write(ZG_OFFS_USRL, data[5]);
dest1[0] = gyroBias[0] / (float)gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction
dest1[1] = gyroBias[1] / (float)gyrosensitivity;
dest1[2] = gyroBias[2] / (float)gyrosensitivity;
// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
// the accelerometer biases calculated above must be divided by 8.
uint[] accelBiasReg = { 0, 0, 0 }; // A place to hold the factory accelerometer trim biases
// Read factory accelerometer trim values
accelBiasReg[0] = _mpu9150.ReadUshort(XA_OFFSET_H);
accelBiasReg[1] = _mpu9150.ReadUshort(YA_OFFSET_H);
accelBiasReg[2] = _mpu9150.ReadUshort(ZA_OFFSET_H);
var mask = 1u;
// Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
byte[] maskBit = { 0, 0, 0 }; // Define array to hold mask bit for each accelerometer bias axis
for (ii = 0; ii < 3; ii++)
{
if ((accelBiasReg[ii] & mask) != 0) //not sure if this should be 0x01 or 0x00?
{
maskBit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
}
}
// Construct total accelerometer bias, including calculated average accelerometer bias from above
accelBiasReg[0] -= (ushort)(accelBias[0] / 8);
// Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
accelBiasReg[1] -= (ushort)(accelBias[1] / 8);
accelBiasReg[2] -= (ushort)(accelBias[2] / 8);
data = new byte[6];
data[0] = (byte)((accelBiasReg[0] >> 8) & 0xFF);
data[1] = (byte)((accelBiasReg[0]) & 0xFF);
data[1] = (byte)(data[1] | maskBit[0]);
// preserve temperature compensation bit when writing back to accelerometer bias registers
data[2] = (byte)((accelBiasReg[1] >> 8) & 0xFF);
data[3] = (byte)((accelBiasReg[1]) & 0xFF);
data[3] = (byte)(data[3] | maskBit[1]);
// preserve temperature compensation bit when writing back to accelerometer bias registers
data[4] = (byte)((accelBiasReg[2] >> 8) & 0xFF);
data[5] = (byte)((accelBiasReg[2]) & 0xFF);
data[5] = (byte)(data[5] | maskBit[2]);
// preserve temperature compensation bit when writing back to accelerometer bias registers
// Apparently this is not working for the acceleration biases in the MPU-9250
// Are we handling the temperature correction bit properly?
// Push accelerometer biases to hardware registers
_mpu9150.Write(XA_OFFSET_H, data[0]);
_mpu9150.Write(XA_OFFSET_L_TC, data[1]);
_mpu9150.Write(YA_OFFSET_H, data[2]);
_mpu9150.Write(YA_OFFSET_L_TC, data[3]);
_mpu9150.Write(ZA_OFFSET_H, data[4]);
_mpu9150.Write(ZA_OFFSET_L_TC, data[5]);
// Output scaled accelerometer biases for manual subtraction in the main program
dest2[0] = accelBias[0] / (float)accelsensitivity;
dest2[1] = accelBias[1] / (float)accelsensitivity;
dest2[2] = accelBias[2] / (float)accelsensitivity;
}
private async void Interrupt(GpioPin sender, GpioPinValueChangedEventArgs args)
{
await Task.Delay(10);
if (_mpu9150 == null)
{
return;
}
int interruptStatus = _mpu9150.ReadRegisterSingle((byte)Mpu9150Setup.InterruptStatus);
if ((interruptStatus & 0x10) != 0)
{
_mpu9150.Write((byte)Mpu9150Setup.UserCtrl, 0x44);// reset - enable fifo
}
if ((interruptStatus & 0x1) == 0)
{
return;
}
var ea = new MpuSensorEventArgs();
ea.Status = (byte)interruptStatus;
ea.SamplePeriod = 0.02f;
var l = new List <MpuSensorValue>();
int count = _mpu9150.ReadUshort((byte)Mpu9150Setup.FifoCount);
while (count >= SensorBytes)
{
_mpu9150.Write((byte)Mpu9150Setup.FifoReadWrite);
byte[] buffer;
_mpu9150.Read(SensorBytes, out buffer);
count -= SensorBytes;
var xa = (short)(buffer[0] << 8 | buffer[1]);
var ya = (short)(buffer[2] << 8 | buffer[3]);
var za = (short)(buffer[4] << 8 | buffer[5]);
var xg = (short)(buffer[6] << 8 | buffer[7]);
var yg = (short)(buffer[8] << 8 | buffer[9]);
var zg = (short)(buffer[10] << 8 | buffer[11]);
var sv = new MpuSensorValue
{
AccelerationX = xa / 16384d,
AccelerationY = ya / 16384d,
AccelerationZ = za / 16384d,
GyroX = xg / 131d,
GyroY = yg / 131d,
GyroZ = zg / 131d
};
l.Add(sv);
var gain = 0.00875;
var xRotationPerSecond = sv.GyroX * gain; //xRotationPerSecond is the rate of rotation per second.
var loopPeriod = 0.015; //loop period - 0.02
_gyroXangle += xRotationPerSecond * loopPeriod;
var radToDeg = 57.29578;
var accXangle = (Math.Atan2(sv.AccelerationY, sv.AccelerationZ) + Math.PI) * radToDeg;
var complementaryFilterConstant = 0.98;
_cFangleX = complementaryFilterConstant * (_cFangleX + xRotationPerSecond * loopPeriod) + (1 - complementaryFilterConstant) * accXangle;
//Debug.WriteLine("X: " + sv.GyroX + ", Y: " + sv.GyroY + ", Z: " + sv.GyroZ);
//Debug.WriteLine("CFangleX: " + _cFangleX);
//Debug.WriteLine("AccelX: " + sv.AccelerationX + ", AccelY: " + sv.AccelerationY + ", AccelZ: " + sv.AccelerationZ);
}
ea.Values = l.ToArray();
if (SensorInterruptEvent == null)
{
return;
}
if (ea.Values.Length > 0)
{
SensorInterruptEvent(this, ea);
}
}