/// <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(); }