/// <summary>
/// Ließt ein Datum
/// </summary>
/// <param name="dataRegister">Das zu lesende Register</param>
/// <returns></returns>
private double ReadingValue(Mpu9250DataRegister dataRegister)
{
byte[] readRegister = new byte[2];
MPU.WriteRead(new byte[] { (byte)dataRegister }, readRegister);
byte tempH = readRegister[0];
byte tempL = readRegister[1];
Int16 tempRaw = (Int16)(tempH << 8 | tempL);
double value = (double)tempRaw;
return(value);
}
public static void RemoveMPU(ref MPU LList, MPU LMember, texmaps.GameBoard gb)
{
//{Locate and extract member LMember from list LList.}
//{Then, dispose of LMember.}
//{Initialize A and B}
MPU B = LList;
MPU A = null;
//{Locate LMember in the list. A will thereafter be either null,}
//{if LMember if first in the list, or it will be equal to the}
//{element directly preceding LMember.}
while (B != LMember && B != null)
{
A = B;
B = B.next;
}
if (B == null)
{
//{Major FUBAR. The member we were trying to remove can't}
//{be found in the list.}
Crt.Write("ERROR- RemoveMPU asked to remove a link that doesnt exist.\n");
do
{
rpgtext.RPGKey();
} while (true);
}
else if (A == null)
{
//{There's no element before the one we want to remove,}
//{i.e. it's the first one in the list.}
LList = B.next;
//{Get rid of the model.}
texmaps.GRemoveModel(B.M, gb);
B = null;
}
else
{
//{We found the attribute we want to delete and have another}
//{one standing before it in line. Go to work.}
A.next = B.next;
//{Get rid of the model.}
texmaps.GRemoveModel(B.M, gb);
B = null;
}
}
/// <summary>
/// Creates a new WRAMP CPU.
/// </summary>
/// <param name="addressBus">The address bus.</param>
/// <param name="dataBus">The data bus.</param>
/// <param name="irqs">The interrupt request lines (IRQs).</param>
/// <param name="cs">The chip-select lines.</param>
public SimpleWrampCpu(Bus addressBus, Bus dataBus, Bus irqs)
{
mAddressBus = addressBus;
mDataBus = dataBus;
mIrqs = irqs;
mGpRegisters = new RegisterFile("General Purpose Registers");
mSpRegisters = new RegisterFile("Special Purpose Registers");
mAlu = new ALU();
mIR = new IR();
mMPU = new MPU(mSpRegisters, mAddressBus, mDataBus);
}
public static MPU AddMPU(ref MPU CList, texmaps.GameBoard gb, int C, int X, int Y)
{
//{Add a MPU to the map at location X,Y.}
//{Allocate memory for IT.}
MPU it = new MPU();
//{Attach IT to the list.}
it.next = CList;
CList = it;
it.kind = C;
it.M = texmaps.GAddModel(gb, MPUGFX, MPUMan[C - 1].color, Crt.Color.White, false, X, Y, MKIND_MPU);
return(it);
}
public static void WriteMPU(MPU CL, StreamWriter f)
{
//{Save the linked list of computers to the file F.}
while (CL != null)
{
f.WriteLine(CL.kind);
//{Record the position of the computer}
f.WriteLine(CL.M.x);
f.WriteLine(CL.M.y);
f.WriteLine(CL.Attr);
CL = CL.next;
}
f.WriteLine(-1);
}
public static MPU LocateMPU(texmodel.Model M, MPU C)
{
//{Given model M, locate the MPU that is being referred to.}
//{Return null if no such MPU can be found.}
MPU it = null;
while (C != null && it == null)
{
if (C.M == M)
{
it = C;
}
C = C.next;
}
return(it);
}
/// <summary>
/// Initialisierung
/// </summary>
public void Init()
{
var settings = new I2cConnectionSettings((int)Address);
settings.BusSpeed = I2cBusSpeed.FastMode;
var controller = I2cController.GetDefaultAsync().GetResults();
MPU = controller.GetDevice(settings);
MPU.Write(new byte[] { (byte)Mpu9250Register.SMPLRT_DIV, SMPLRT_20Hz });
MPU.Write(new byte[] { (byte)Mpu9250Register.CONFIG, CONFIG_94Hz });
MPU.Write(new byte[] { (byte)Mpu9250Register.GYRO_CONFIG, GYRO_CONFIG_500 });
MPU.Write(new byte[] { (byte)Mpu9250Register.ACCEL_CONFIG, ACCEL_CONFIG_4g });
MPU.Write(new byte[] { (byte)Mpu9250Register.INT_PIN_CFG, BYPASS_ENABLED });
MPU.Write(new byte[] { (byte)Mpu9250Register.PWR_MGMT_1, PWR_MNGMT_ZCLOCK });
Calibrate();
}
public static MPU ReadMPU(StreamReader f, texmaps.GameBoard gb)
{
//{Read a list of computers from disk.}
//{Initialize the list to NIL.}
MPU CList = null;
//{Keep reading data until we get a termination value, -1.}
int N = int.Parse(f.ReadLine());
while (N != -1)
{
//{Read the rest of the cloud data.}
int X = int.Parse(f.ReadLine());
int Y = int.Parse(f.ReadLine());
//{Add this computer to the list.}
MPU Current = AddMPU(ref CList, gb, N, X, Y);
Current.Attr = f.ReadLine();
N = int.Parse(f.ReadLine());
}
return(CList);
}
/// <summary>
/// Aktualisierung der Daten
/// </summary>
public void Update()
{
Mpu9250Data = MPU.Read();
}
/// <summary>
/// Kalibrieren
/// </summary>
public void Calibrate()
{
// reset device
// Write a one to bit 7 reset bit; toggle reset device
MPU.Write(new byte[] { (byte)Mpu9250Register.PWR_MGMT_1, 0x80 });
Thread.Sleep(100);
// get stable time source; Auto select clock source to be PLL gyroscope
// reference if ready else use the internal oscillator, bits 2:0 = 001
MPU.Write(new byte[] { (byte)Mpu9250Register.PWR_MGMT_1, 0x01 });
MPU.Write(new byte[] { (byte)Mpu9250Register.PWR_MGMT_2, 0x00 });
Thread.Sleep(200);
// Configure device for bias calculation
MPU.Write(new byte[] { (byte)Mpu9250Register.INT_ENABLE, 0x00 }); // Disable all interrupts
MPU.Write(new byte[] { (byte)Mpu9250Register.FIFO_EN, 0x00 }); // Disable FIFO
MPU.Write(new byte[] { (byte)Mpu9250Register.PWR_MGMT_1, 0x00 }); // Turn on internal clock source
MPU.Write(new byte[] { (byte)Mpu9250Register.I2C_MST_CTRL, 0x00 }); // Disable I2C master
MPU.Write(new byte[] { (byte)Mpu9250Register.USER_CTRL, 0x00 }); // Disable FIFO and I2C master modes
MPU.Write(new byte[] { (byte)Mpu9250Register.USER_CTRL, 0x0C }); // Reset FIFO and DMP
Thread.Sleep(15);
// Configure MPU6050 gyro and accelerometer for bias calculation
MPU.Write(new byte[] { (byte)Mpu9250Register.CONFIG, 0x01 }); // Set low-pass filter to 188 Hz
MPU.Write(new byte[] { (byte)Mpu9250Register.SMPLRT_DIV, 0x00 }); // Set sample rate to 1 kHz
MPU.Write(new byte[] { (byte)Mpu9250Register.GYRO_CONFIG, 0x00 }); // Set gyro full-scale to 250 degrees per second, maximum sensitivity
MPU.Write(new byte[] { (byte)Mpu9250Register.ACCEL_CONFIG, 0x00 }); // Set accelerometer full-scale to 2 g, maximum sensitivity
// Configure FIFO to capture accelerometer and gyro data for bias calculation
MPU.Write(new byte[] { (byte)Mpu9250Register.USER_CTRL, 0x40 }); // Enable FIFO
MPU.Write(new byte[] { (byte)Mpu9250Register.FIFO_EN, 0x78 }); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150)
Thread.Sleep(40); // accumulate 40 samples in 40 milliseconds = 480 bytes
// At end of sample accumulation, turn off FIFO sensor read
var data = new byte[2];
MPU.Write(new byte[] { (byte)Mpu9250Register.FIFO_EN, 0x00 }); // Disable gyro and accelerometer sensors for FIFO
MPU.WriteRead(new byte[] { (byte)Mpu9250Register.FIFO_COUNTH }, data); // read FIFO sample count
var fifo_count = (data[0] << 8) | data[1];
var packet_count = fifo_count / 12; // How many sets of full gyro and accelerometer data for averaging
data = new byte[12];
int[] gyro_bias = { 0, 0, 0 }, accel_bias = { 0, 0, 0 };
for (int i = 0; i < packet_count; i++)
{
int[] accel_temp = { 0, 0, 0 }, gyro_temp = { 0, 0, 0 };
MPU.WriteRead(new byte[] { (byte)Mpu9250Register.FIFO_R_W }, data); // read data for averaging
accel_temp[0] = ((data[0] << 8) | data[1]); // Form signed 16-bit integer for each sample in FIFO
accel_temp[1] = ((data[2] << 8) | data[3]);
accel_temp[2] = ((data[4] << 8) | data[5]);
gyro_temp[0] = ((data[6] << 8) | data[7]);
gyro_temp[1] = ((data[8] << 8) | data[9]);
gyro_temp[2] = ((data[10] << 8) | data[11]);
accel_bias[0] += accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
accel_bias[1] += accel_temp[1];
accel_bias[2] += accel_temp[2];
gyro_bias[0] += gyro_temp[0];
gyro_bias[1] += gyro_temp[1];
gyro_bias[2] += gyro_temp[2];
}
accel_bias[0] /= packet_count; // Normalize sums to get average count biases
accel_bias[1] /= packet_count;
accel_bias[2] /= packet_count;
gyro_bias[0] /= packet_count;
gyro_bias[1] /= packet_count;
gyro_bias[2] /= packet_count;
var accelsensitivity = 16384; // = 16384 LSB/g
if (accel_bias[2] > 0L)
{
// Remove gravity from the z-axis accelerometer bias calculation
accel_bias[2] -= accelsensitivity;
}
else
{
accel_bias[2] += accelsensitivity;
}
// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
data[0] = (byte)((-gyro_bias[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)((-gyro_bias[0] / 4) & 0xFF); // Biases are additive, so change sign on calculated average gyro biases
data[2] = (byte)((-gyro_bias[1] / 4 >> 8) & 0xFF);
data[3] = (byte)((-gyro_bias[1] / 4) & 0xFF);
data[4] = (byte)((-gyro_bias[2] / 4 >> 8) & 0xFF);
data[5] = (byte)((-gyro_bias[2] / 4) & 0xFF);
// Push gyro biases to hardware registers
MPU.Write(new byte[] { (byte)Mpu9250Register.XG_OFFSET_H, data[0] });
MPU.Write(new byte[] { (byte)Mpu9250Register.XG_OFFSET_L, data[1] });
MPU.Write(new byte[] { (byte)Mpu9250Register.YG_OFFSET_H, data[2] });
MPU.Write(new byte[] { (byte)Mpu9250Register.YG_OFFSET_L, data[3] });
MPU.Write(new byte[] { (byte)Mpu9250Register.ZG_OFFSET_H, data[4] });
MPU.Write(new byte[] { (byte)Mpu9250Register.ZG_OFFSET_L, data[5] });
// Output scaled gyro biases for display in the main program
var gyrosensitivity = 131; // = 131 LSB/degrees/sec
gyroBias[0] = (float)gyro_bias[0] / (float)gyrosensitivity;
gyroBias[1] = (float)gyro_bias[1] / (float)gyrosensitivity;
gyroBias[2] = (float)gyro_bias[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.
int[] accel_bias_reg = { 0, 0, 0 }; // A place to hold the factory accelerometer trim biases
data = new byte[2];
MPU.WriteRead(new byte[] { (byte)Mpu9250Register.XA_OFFSET_H }, data); // Read factory accelerometer trim values
accel_bias_reg[0] = ((data[0] << 8) | data[1]);
MPU.WriteRead(new byte[] { (byte)Mpu9250Register.YA_OFFSET_H }, data);
accel_bias_reg[1] = ((data[0] << 8) | data[1]);
MPU.WriteRead(new byte[] { (byte)Mpu9250Register.ZA_OFFSET_H }, data);
accel_bias_reg[2] = ((data[0] << 8) | data[1]);
var mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
int[] mask_bit = { 0, 0, 0 }; // Define array to hold mask bit for each accelerometer bias axis
for (int i = 0; i < 3; i++)
{
if (((ulong)accel_bias_reg[i] & mask) != 0)
{
mask_bit[i] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
}
}
// Construct total accelerometer bias, including calculated average accelerometer bias from above
accel_bias_reg[0] -= (accel_bias[0] / 8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
accel_bias_reg[1] -= (accel_bias[1] / 8);
accel_bias_reg[2] -= (accel_bias[2] / 8);
data = new byte[6];
data[0] = (byte)((accel_bias_reg[0] >> 8) & 0xFF);
data[1] = (byte)((accel_bias_reg[0]) & 0xFF);
data[1] = (byte)(data[1] | mask_bit[0]); // preserve temperature compensation bit when writing back to accelerometer bias registers
data[2] = (byte)((accel_bias_reg[1] >> 8) & 0xFF);
data[3] = (byte)((accel_bias_reg[1]) & 0xFF);
data[3] = (byte)(data[3] | mask_bit[1]); // preserve temperature compensation bit when writing back to accelerometer bias registers
data[4] = (byte)((accel_bias_reg[2] >> 8) & 0xFF);
data[5] = (byte)((accel_bias_reg[2]) & 0xFF);
data[5] = (byte)(data[5] | mask_bit[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
MPU.Write(new byte[] { (byte)Mpu9250Register.XA_OFFSET_H, data[0] });
MPU.Write(new byte[] { (byte)Mpu9250Register.XA_OFFSET_L, data[1] });
MPU.Write(new byte[] { (byte)Mpu9250Register.YA_OFFSET_H, data[2] });
MPU.Write(new byte[] { (byte)Mpu9250Register.YA_OFFSET_L, data[3] });
MPU.Write(new byte[] { (byte)Mpu9250Register.ZA_OFFSET_H, data[4] });
MPU.Write(new byte[] { (byte)Mpu9250Register.ZA_OFFSET_L, data[5] });
// Output scaled accelerometer biases for display in the main program
accelBias[0] = (float)accel_bias[0] / (float)accelsensitivity;
accelBias[1] = (float)accel_bias[1] / (float)accelsensitivity;
accelBias[2] = (float)accel_bias[2] / (float)accelsensitivity;
}
/// <summary>
/// Bereinigung
/// </summary>
public void Dispose()
{
MPU.Dispose();
}
