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