void HandleQuat()
{
quat = new double[4];
while (working)
{
if (!CoordenaDados.circularBufferQ.IsEmpty)
{
quat = CoordenaDados.HandleQuat();
}
//show values
string resultado =
"\n W: " + quat[0].ToString("0.00") +
"\n X: " + quat[1].ToString("0.00") +
"\n Y: " + quat[2].ToString("0.00") +
"\n Z: " + quat[3].ToString("0.00");
this.lbQAtual.BeginInvoke(new Action(() => this.lbQAtual.Text = resultado));
}
Console.WriteLine("Quat encerrado! ");
}
/// <summary>
/// Recebe os valores de quat armazenados
/// </summary>
void HandleQuat()
{
quat = new double[4];
double tq = 0;
while (working)
{
if (!CoordenaDados.circularBufferQ.IsEmpty)
{
quat = CoordenaDados.HandleQuat();
//corrigindo W
quat[0] = Math.Abs(Math.Sqrt(Math.Pow(quat[1], 2) + Math.Pow(quat[2], 2) + Math.Pow(quat[3], 2)));
quat[0] = Math.Cos(Math.Asin(quat[0]));
fileQuat.WriteLine(quat[0] + "\t" + quat[1] + "\t" + quat[2] + "\t" + quat[3]);
//show values
lbPhi.Invoke(new Action(() => lbPhi.Text = "W: " + quat[0].ToString("0.000")));
lbTheta.Invoke(new Action(() => lbTheta.Text = "X: " + quat[1].ToString("0.000")));
lbPsi.Invoke(new Action(() => lbPsi.Text = "Y: " + quat[2].ToString("0.000")));
lbQuatZ.Invoke(new Action(() => lbQuatZ.Text = "Z: " + quat[3].ToString("0.000")));
//trackBar_phi.Invoke(new Action(() => trackBar_phi.Value = (int)(quat[0] * 100.0)));
//trackBar_theta.Invoke(new Action(() => trackBar_theta.Value = (int)(quat[1] * 100.0)));
//trackBar_psi.Invoke(new Action(() => trackBar_psi.Value = (int)(quat[2] * 100.0)));
//trackBar_Z.Invoke(new Action(() => trackBar_Z.Value = (int)(quat[3] * 100.0)));
//show values
string resultado =
"\n W: " + quat[0].ToString("0.00") +
"\n X: " + quat[1].ToString("0.00") +
"\n Y: " + quat[2].ToString("0.00") +
"\n Z: " + quat[3].ToString("0.00");
lbQAtual.BeginInvoke(new Action(() => lbQAtual.Text = resultado));
//switch (estadoAtual)
//{
// case "cima":
// this.pictureBox1.Invoke(new Action(() => this.pictureBox1.Image = Properties.Resources.cima));
// break;
// case "baixo":
// this.pictureBox1.Invoke(new Action(() => this.pictureBox1.Image = Properties.Resources.baixo));
// break;
// case "direita":
// this.pictureBox1.Invoke(new Action(() => this.pictureBox1.Image = Properties.Resources.direita));
// break;
// case "esquerda":
// this.pictureBox1.Invoke(new Action(() => this.pictureBox1.Image = Properties.Resources.esquerda));
// break;
// default:
// this.pictureBox1.Invoke(new Action(() => this.pictureBox1.Image = null));
// break;
//}
SetDirecao();
}
}
string timestamp = DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss.fff", CultureInfo.InvariantCulture);
fileQuat.WriteLine(timestamp);
fileQuat.Close();
Console.WriteLine("Quat encerrado! ");
}
