예제 #1
0
파일: Main.cs 프로젝트: amirivojdan/Pro-Imu
        private void dataGridView_device_data_CellEndEdit(object sender, DataGridViewCellEventArgs e)
        {
            var register = Mpu6050.GetRegisterFromDescription(
                dataGridView_device_data.Rows[e.RowIndex].Cells["description"].Value.ToString());

            var data = (float)Convert.ToDouble(dataGridView_device_data.Rows[e.RowIndex].Cells["value"].Value);

            if (Mpu6050.RegisterLength(register) == 4)
            {
                var floatValue = new ByteArrayFloat();
                floatValue.FloatNumber = data;
                _mpu6050.WriteData(_deviceId, (int)register, new List <Byte> {
                    floatValue.Byte0, floatValue.Byte1, floatValue.Byte2, floatValue.Byte3
                }, true);
            }
            else if (Mpu6050.RegisterLength(register) == 2)
            {
                _mpu6050.WriteWord(_deviceId, (int)register, (int)data, true);
            }
            else
            {
                _mpu6050.WriteByte(_deviceId, (int)register, (int)data, true);
            }


            RefreshRegisters(_deviceId);
        }
예제 #2
0
        public void UpdateFilteredAngles(int id)
        {
            lock (_key)
            {
                var data = ReadData(id, Register.FilteredQ0, Register.FilteredAngleYaw);

                ByteArrayFloat q0 = new ByteArrayFloat();
                q0.Byte0 = data[0];
                q0.Byte1 = data[1];
                q0.Byte2 = data[2];
                q0.Byte3 = data[3];

                ByteArrayFloat q1 = new ByteArrayFloat();
                q1.Byte0 = data[4];
                q1.Byte1 = data[5];
                q1.Byte2 = data[6];
                q1.Byte3 = data[7];

                ByteArrayFloat q2 = new ByteArrayFloat();
                q2.Byte0 = data[8];
                q2.Byte1 = data[9];
                q2.Byte2 = data[10];
                q2.Byte3 = data[11];

                ByteArrayFloat q3 = new ByteArrayFloat();
                q3.Byte0 = data[12];
                q3.Byte1 = data[13];
                q3.Byte2 = data[14];
                q3.Byte3 = data[15];


                ByteArrayFloat roll = new ByteArrayFloat();
                roll.Byte0 = data[16];
                roll.Byte1 = data[17];
                roll.Byte2 = data[18];
                roll.Byte3 = data[19];

                ByteArrayFloat pitch = new ByteArrayFloat();
                pitch.Byte0 = data[20];
                pitch.Byte1 = data[21];
                pitch.Byte2 = data[22];
                pitch.Byte3 = data[23];

                ByteArrayFloat yaw = new ByteArrayFloat();
                yaw.Byte0 = data[24];
                yaw.Byte1 = data[25];
                yaw.Byte2 = data[26];
                yaw.Byte3 = data[27];

                Q0 = q0.FloatNumber;
                Q1 = q1.FloatNumber;
                Q2 = q2.FloatNumber;
                Q3 = q3.FloatNumber;

                FilteredAngleRoll  = roll.FloatNumber * 180 / Math.PI;
                FilteredAnglePitch = pitch.FloatNumber * 180 / Math.PI;
                FilteredAngleYaw   = yaw.FloatNumber * 180 / Math.PI;
            }
        }
예제 #3
0
 public void SetBeta(int id, float value)
 {
     lock (_key)
     {
         var valuetoByte = new ByteArrayFloat();
         valuetoByte.FloatNumber = value;
         WriteData(id, (int)Register.Beta,
                   new List <byte> {
             valuetoByte.Byte0, valuetoByte.Byte1, valuetoByte.Byte2, valuetoByte.Byte3
         }, true);
     }
 }
예제 #4
0
 public float GetBeta(int id)
 {
     lock (_key)
     {
         var data = ReadData(id, Register.Beta, 4);
         var beta = new ByteArrayFloat();
         beta.Byte0 = data[0];
         beta.Byte1 = data[1];
         beta.Byte2 = data[2];
         beta.Byte3 = data[3];
         return(beta.FloatNumber);
     }
 }
예제 #5
0
        public void UpdateAll(int id)
        {
            lock (_key)
            {
                var data = ReadData(id, Register.RawAccelX, Register.FilteredAngleYaw);

                RawAccelX = (short)((data[0] << 8) + data[1]);
                RawAccelY = (short)((data[2] << 8) + data[3]);
                RawAccelZ = (short)((data[4] << 8) + data[5]);

                //temp = (short)(data[6] << 8 + data[7]);

                RawGyroX = (short)((data[8] << 8) + data[9]);
                RawGyroY = (short)((data[10] << 8) + data[11]);
                RawGyroZ = (short)((data[12] << 8) + data[13]);

                ByteArrayFloat q0 = new ByteArrayFloat();
                q0.Byte0 = data[14];
                q0.Byte1 = data[15];
                q0.Byte2 = data[16];
                q0.Byte3 = data[17];

                ByteArrayFloat q1 = new ByteArrayFloat();
                q1.Byte0 = data[18];
                q1.Byte1 = data[19];
                q1.Byte2 = data[20];
                q1.Byte3 = data[21];

                ByteArrayFloat q2 = new ByteArrayFloat();
                q2.Byte0 = data[22];
                q2.Byte1 = data[23];
                q2.Byte2 = data[24];
                q2.Byte3 = data[25];

                ByteArrayFloat q3 = new ByteArrayFloat();
                q3.Byte0 = data[26];
                q3.Byte1 = data[27];
                q3.Byte2 = data[28];
                q3.Byte3 = data[29];


                ByteArrayFloat roll = new ByteArrayFloat();
                roll.Byte0 = data[30];
                roll.Byte1 = data[31];
                roll.Byte2 = data[32];
                roll.Byte3 = data[33];

                ByteArrayFloat pitch = new ByteArrayFloat();
                pitch.Byte0 = data[34];
                pitch.Byte1 = data[35];
                pitch.Byte2 = data[36];
                pitch.Byte3 = data[37];

                ByteArrayFloat yaw = new ByteArrayFloat();
                yaw.Byte0 = data[38];
                yaw.Byte1 = data[39];
                yaw.Byte2 = data[40];
                yaw.Byte3 = data[41];

                Q0 = q0.FloatNumber;
                Q1 = q1.FloatNumber;
                Q2 = q2.FloatNumber;
                Q3 = q3.FloatNumber;

                FilteredAngleRoll  = roll.FloatNumber * 180 / Math.PI;
                FilteredAnglePitch = pitch.FloatNumber * 180 / Math.PI;
                FilteredAngleYaw   = yaw.FloatNumber * 180 / Math.PI;
            }
        }