/// <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(); }
/// <summary> /// Executes the current instruction. /// </summary> private void ExecuteInstruction() { mAddressBus.IsWrite = false; mSpRegisters[RegisterFile.SpRegister.icount]++; try { switch (mIR.OpCode) { case IR.Opcode.arith_r: case IR.Opcode.test_r_special: if (mIR.OpCode == IR.Opcode.test_r_special && (mIR.Func == IR.Function.movgs || mIR.Func == IR.Function.movsg || mIR.Func == IR.Function.lhi)) { switch (mIR.Func) { case IR.Function.movgs: //break mInterruptStatus |= (uint)ExceptionSource.BREAK; break; case IR.Function.movsg: //syscall mInterruptStatus |= (uint)ExceptionSource.SYSCALL; break; case IR.Function.lhi: //rfe ProcessRfe(); break; } } else { mAlu.Func = mIR.Func; mAlu.Rs = mGpRegisters[(RegisterFile.GpRegister)mIR.Rs]; mAlu.Rt = mGpRegisters[(RegisterFile.GpRegister)mIR.Rt]; mGpRegisters[(RegisterFile.GpRegister)mIR.Rd] = mAlu.Result; } break; case IR.Opcode.arith_i: case IR.Opcode.test_i_special: if (mIR.OpCode == IR.Opcode.test_i_special && (mIR.Func == IR.Function.movgs || mIR.Func == IR.Function.movsg)) { //Make sure we are in kernel mode! if ((mSpRegisters[RegisterFile.SpRegister.cctrl] & 0x00000008) == 0) { mInterruptStatus |= (uint)ExceptionSource.GPF; } else { switch (mIR.Func) { case IR.Function.movgs: mSpRegisters[(RegisterFile.SpRegister)mIR.Rd] = mGpRegisters[(RegisterFile.GpRegister)mIR.Rs]; break; case IR.Function.movsg: mGpRegisters[(RegisterFile.GpRegister)mIR.Rd] = mSpRegisters[(RegisterFile.SpRegister)mIR.Rs]; break; } } } else { mAlu.Func = mIR.Func; mAlu.Rs = mGpRegisters[(RegisterFile.GpRegister)mIR.Rs]; mAlu.Rt = mIR.Immed16; if ((((uint)mIR.Func) & 1) == 0) //if this instruction is signed, sign-extend it { if ((mAlu.Rt & 0x8000) != 0) { mAlu.Rt |= 0xFFFF0000; } } mGpRegisters[(RegisterFile.GpRegister)mIR.Rd] = mAlu.Result; } break; case IR.Opcode.j: PC = mIR.Immed20; break; case IR.Opcode.jr: PC = mGpRegisters[(RegisterFile.GpRegister)mIR.Rs]; break; case IR.Opcode.jal: mGpRegisters[RegisterFile.GpRegister.ra] = PC; goto case IR.Opcode.j; //OK, I used a goto. Shoot me. case IR.Opcode.jalr: mGpRegisters[RegisterFile.GpRegister.ra] = PC; goto case IR.Opcode.jr; //OK, I used a goto. Shoot me. case IR.Opcode.lw: mMPU.Write((uint)(mGpRegisters[(RegisterFile.GpRegister)mIR.Rs] + mIR.SignedImmed20) & 0xfffff); mGpRegisters[(RegisterFile.GpRegister)mIR.Rd] = mDataBus.Value; break; case IR.Opcode.sw: mMPU.Write((uint)(mGpRegisters[(RegisterFile.GpRegister)mIR.Rs] + mIR.SignedImmed20) & 0xfffff); mAddressBus.IsWrite = true; mDataBus.Write(mGpRegisters[(RegisterFile.GpRegister)mIR.Rd]); break; case IR.Opcode.beqz: if (mGpRegisters[(RegisterFile.GpRegister)mIR.Rs] == 0) { PC = (uint)(PC + mIR.SignedImmed20); } break; case IR.Opcode.bnez: if (mGpRegisters[(RegisterFile.GpRegister)mIR.Rs] != 0) { PC = (uint)(PC + mIR.SignedImmed20); } break; case IR.Opcode.la: mGpRegisters[(RegisterFile.GpRegister)mIR.Rd] = mIR.Immed20; break; default: //Unknown op-code, so throw up a GPF mInterruptStatus |= (uint)ExceptionSource.GPF; break; } } catch (DivideByZeroException) { mInterruptStatus |= (uint)ExceptionSource.ARITH; } catch (OverflowException) { mInterruptStatus |= (uint)ExceptionSource.ARITH; } catch (AccessViolationException) { // Thrown by lw or sw if we are in user mode and code attempts to // access memory restricted by $ptable. mInterruptStatus |= (uint)ExceptionSource.GPF; } }
/// <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; }