/// <summary>
/// Quaternion data received event to set servo angles of gimbal.
/// </summary>
void xIMUserial_QuaternionDataReceived(object sender, x_IMU_API.QuaternionData e)
{
x_IMU_API.QuaternionData quaternionData;
if (radioButton_controlMode.Checked)
{
quaternionData = e;
}
else
{
quaternionData = e.ConvertToConjugate();
}
try
{
float[] EulerAngles = quaternionData.ConvertToEulerAngles();
gimbalSerial.SetTilt(EulerAngles[0]);
gimbalSerial.SetRoll(EulerAngles[1]);
gimbalSerial.SetPan(EulerAngles[2]);
}
catch { }
}
private void timer1_Tick(object sender, EventArgs e)
{
a = (float)Math.Cos((alpha * Math.PI) / (180 * 2));
b = (float)Math.Sin((alpha * Math.PI) / (180 * 2));
float[] Quaternion_1 = new float[] { a, 0.0f, b, .0f };
float[] quaternion = new float[] { 1, 0.0f, 0.0f, 0.0f };
float[] eulerAngles = new float[] { 0.0f, 0.0f, 0.0f };
Quaternion_1[0] = a;
Quaternion_1[1] = 0.0f;
Quaternion_1[2] = b;
Quaternion_1[3] = 0.0f;
qw.Quaternion = Quaternion_1;
/* Refresh rotate matrix on the form object. */
//Form_3D_DA.RotationMatrix = qw.ConvertToRotationMatrix();
#if DEMO
Quaternion_1[0] = a;
Quaternion_1[1] = b;
Quaternion_1[2] = 0.0f;
Quaternion_1[3] = 0.0f;
qw.Quaternion = Quaternion_1;
Form_3D_DB.RotationMatrix = qw.ConvertToRotationMatrix();
#endif
#if ! ACCEL
if (Graph_2D_DA != null)
{
Graph_2D_DA.DrawGraph(
(double)Port_1.currentReceivedData.ExternalBoardSystemTime.ui32,
(double)Port_1.currentReceivedData.Accelerometer.Axis_X.i16,
0,
10000.0,
200,
zGMaxAmountOfPoints
);
Graph_2D_DA.DrawGraph(
(double)Port_1.currentReceivedData.ExternalBoardSystemTime.ui32,
(double)Port_1.currentReceivedData.Accelerometer.Axis_Y.i16,
1,
10000.0,
200,
zGMaxAmountOfPoints
);
Graph_2D_DA.DrawGraph(
(double)Port_1.currentReceivedData.ExternalBoardSystemTime.ui32,
(double)Port_1.currentReceivedData.Accelerometer.Axis_Z.i16,
2,
10000.0,
200,
zGMaxAmountOfPoints
);
}
#else //GYRO
if (Graph_2D_DA != null)
{
Graph_2D_DA.DrawGraph(
(double)Port_1.currentReceivedData.ExternalBoardSystemTime.ui32,
(double)Port_1.currentReceivedData.Gyro.Rot_XY.i16 -2 ,
0,
10000.0,
200,
zGMaxAmountOfPoints
);
Graph_2D_DA.DrawGraph(
(double)Port_1.currentReceivedData.ExternalBoardSystemTime.ui32,
(double)Port_1.currentReceivedData.Gyro.Rot_XZ.i16,
1,
10000.0,
200,
zGMaxAmountOfPoints
);
Graph_2D_DA.DrawGraph(
(double)Port_1.currentReceivedData.ExternalBoardSystemTime.ui32,
(double)Port_1.currentReceivedData.Gyro.Rot_YZ.i16,
2,
10000.0,
200,
zGMaxAmountOfPoints
);
}
#endif
#if ! Gyro
if (Graph_2D_DB != null)
{
Graph_2D_DB.DrawGraph(
(double)Port_1.currentReceivedData.ExternalBoardSystemTime.ui32,
(double)Port_1.currentReceivedData.Magnetometer.Axis_X.i16,
0,
10000.0,
200,
zGMaxAmountOfPoints
);
Graph_2D_DB.DrawGraph(
(double)Port_1.currentReceivedData.ExternalBoardSystemTime.ui32,
(double)Port_1.currentReceivedData.Magnetometer.Axis_Y.i16,
1,
10000.0,
200,
zGMaxAmountOfPoints
);
Graph_2D_DB.DrawGraph(
(double)Port_1.currentReceivedData.ExternalBoardSystemTime.ui32,
(double)Port_1.currentReceivedData.Magnetometer.Axis_Z.i16,
2,
10000.0,
200,
zGMaxAmountOfPoints
);
}
#else //GYRO
if (Graph_2D_DB != null)
{
Graph_2D_DB.DrawGraph(
(double)Port_1.currentReceivedData.ExternalBoardSystemTime.ui32,
(double)Port_1.currentReceivedData.Gyro.Rot_XY.i16 - 2,
0,
10000.0,
200,
zGMaxAmountOfPoints
);
Graph_2D_DB.DrawGraph(
(double)Port_1.currentReceivedData.ExternalBoardSystemTime.ui32,
(double)Port_1.currentReceivedData.Gyro.Rot_XZ.i16,
1,
10000.0,
200,
zGMaxAmountOfPoints
);
Graph_2D_DB.DrawGraph(
(double)Port_1.currentReceivedData.ExternalBoardSystemTime.ui32,
(double)Port_1.currentReceivedData.Gyro.Rot_YZ.i16,
2,
10000.0,
200,
zGMaxAmountOfPoints
);
}
#endif
qw.Quaternion = Port_1.bodyQuaternion;
quaternion = Port_1.bodyQuaternion;
label2.Text = Convert.ToString(Port_1.deltaTime);
eulerAngles = qw.ConvertToEulerAngles();
/******** My Euler Angle ****************************/
float Pitch_Real=0;
float Roll_Real=0;
float Yaw_Real;
Pitch_Real = (float)Math.Atan2 (2*(quaternion[0]*quaternion[1]+quaternion[2]*quaternion[3]),1-2*(quaternion[1]*quaternion[1]+quaternion[2]*quaternion[2]));
Roll_Real = (float)Math.Asin (2*(quaternion[0]*quaternion[2]-quaternion[3]*quaternion[1]));
Yaw_Real = (float)Math.Atan2(2 * (quaternion[0] * quaternion[3] + quaternion[1] * quaternion[2]), 1 - 2 * (quaternion[2] * quaternion[2] + quaternion[3] * quaternion[3]));
label76.Text = Convert.ToString(Roll_Real * 180 / (float)Math.PI); // Roll
label77.Text = Convert.ToString(Pitch_Real * 180 / (float)Math.PI); // Pitch
label78.Text = Convert.ToString(Yaw_Real * 180 / (float)Math.PI); // Yaw
/********************************************************************************/
/****** One more My Euler Angle ************/
float heading;
float attitude;
float bank;
double test = quaternion[1]*quaternion[2] + quaternion[3]*quaternion[0];
if (test > 0.499)
{ // singularity at north pole
heading = 2 * (float)Math.Atan2(quaternion[1],quaternion[0]);
attitude = (float)Math.PI/2;
bank = 0;
}
if (test < -0.499)
{ // singularity at south pole
heading = -2 * (float)Math.Atan2(quaternion[1],quaternion[0]);
attitude = - (float)Math.PI/2;
bank = 0;
}
double sqx = quaternion[1]*quaternion[1];
double sqy = quaternion[2]*quaternion[2];
double sqz = quaternion[3]*quaternion[3];
heading = (float)Math.Atan2(2*quaternion[2]*quaternion[0]-2*quaternion[1]*quaternion[3] , 1 - 2*sqy - 2*sqz);
attitude = (float)Math.Asin(2*test);
bank = (float)Math.Atan2(2 * quaternion[1] * quaternion[0] - 2 * quaternion[2] * quaternion[3], 1 - 2 * sqx - 2 * sqz);
label80.Text = Convert.ToString(heading * 180 / (float)Math.PI); // Pitch
label81.Text = Convert.ToString(bank * 180 / (float)Math.PI); // Roll
label82.Text = Convert.ToString(attitude * 180 / (float)Math.PI); // Yaw
/*********************************************/
/********* Angle Axis ***********************/
float angle;
float x,y,z;
angle = 2 * (float)Math.Acos(Port_1.bodyQuaternion[0]);
double s = Math.Sqrt(1 - Port_1.bodyQuaternion[0] * Port_1.bodyQuaternion[0]);
if (s < 0.001)
{
x = Port_1.bodyQuaternion[1]; // if it is important that axis is normalised then replace with x=1; y=z=0;
y = Port_1.bodyQuaternion[2];
z = Port_1.bodyQuaternion[3];
}
else
{
x = Port_1.bodyQuaternion[1]/(float)s;
y = Port_1.bodyQuaternion[2]/(float)s;
z = Port_1.bodyQuaternion[3]/(float)s;
}
angle = angle * 180 / (float)Math.PI;
label71.Text = Convert.ToString(angle);
label72.Text = Convert.ToString(x);
label73.Text = Convert.ToString(y);
label74.Text = Convert.ToString(z);
/****** End Angle Axis *********************************/
/* Quaternion PC. */
label16.Text = Convert.ToString(eulerAngles[0]); // Roll
label17.Text = Convert.ToString(eulerAngles[1]); // Pitch
label18.Text = Convert.ToString(eulerAngles[2]); // Yaw
label26.Text = Convert.ToString(Port_1.currentReceivedData.EulerAngle_0.f32);
label27.Text = Convert.ToString(Port_1.currentReceivedData.EulerAngle_1.f32);
label28.Text = Convert.ToString(Port_1.currentReceivedData.EulerAngle_2.f32);
/* Add PIDs */
/* Static PID */
/* Roll. */
label39.Text = Convert.ToString(Port_1.currentReceivedData.Roll.kP.u16);
label40.Text = Convert.ToString(Port_1.currentReceivedData.Roll.kI.u16);
label41.Text = Convert.ToString(Port_1.currentReceivedData.Roll.kD.u16);
/* Pitch. */
label42.Text = Convert.ToString(Port_1.currentReceivedData.Pitch.kP.u16);
label43.Text = Convert.ToString(Port_1.currentReceivedData.Pitch.kI.u16);
label44.Text = Convert.ToString(Port_1.currentReceivedData.Pitch.kD.u16);
/* Yaw. */
label45.Text = Convert.ToString(Port_1.currentReceivedData.Yaw.kP.u16);
label46.Text = Convert.ToString(Port_1.currentReceivedData.Yaw.kI.u16);
label47.Text = Convert.ToString(Port_1.currentReceivedData.Yaw.kD.u16);
/* Dynamic PID */
/* Roll. */
label48.Text = Convert.ToString(Port_1.currentReceivedData.Roll.kP_dynamic.u16);
label49.Text = Convert.ToString(Port_1.currentReceivedData.Roll.kI_dynamic.u16);
label50.Text = Convert.ToString(Port_1.currentReceivedData.Roll.kD_dynamic.u16);
/* Pitch. */
label51.Text = Convert.ToString(Port_1.currentReceivedData.Pitch.kP_dynamic.u16);
label52.Text = Convert.ToString(Port_1.currentReceivedData.Pitch.kI_dynamic.u16);
label53.Text = Convert.ToString(Port_1.currentReceivedData.Pitch.kD_dynamic.u16);
/* Yaw. */
label54.Text = Convert.ToString(Port_1.currentReceivedData.Yaw.kP_dynamic.u16);
label55.Text = Convert.ToString(Port_1.currentReceivedData.Yaw.kI_dynamic.u16);
label56.Text = Convert.ToString(Port_1.currentReceivedData.Yaw.kD_dynamic.u16);
#if ! MAGNETOMETER
if (Graph_2D_DC != null)
{
Graph_2D_DC.DrawGraph(
(double)Port_1.currentReceivedData.ExternalBoardSystemTime.ui32,
(double)eulerAngles[0],
0,
100000.0,
200,
3000//zGMaxAmountOfPoints
);
Graph_2D_DC.DrawGraph(
(double)Port_1.currentReceivedData.ExternalBoardSystemTime.ui32,
(double)eulerAngles[1],
1,
100000.0,
200,
3000//zGMaxAmountOfPoints
);
/* midle filter.*/
float angl_sum=0;
int angl_ind=50;
angl_sum = 0;
angl_ind = 0;
while(angl_ind < 10 )
{
angl_sum = angl_sum + angl_midl[angl_ind];
angl_ind++;
}
angl_sum = angl_sum /(float) 10.0;
/******************/
Graph_2D_DC.DrawGraph(
(double)Port_1.currentReceivedData.ExternalBoardSystemTime.ui32,
// ((double)eulerAngles[2] + angl_sum) / 51.0,
((double)angl_sum),
2,
100000.0,
200,
3000//zGMaxAmountOfPoints
);
/* midle filter.*/
angl_sum = 0;
angl_ind = 9;
while (angl_ind > 0)
{
angl_midl[angl_ind] = angl_midl[angl_ind-1];
angl_ind--;
}
//angl_midl[0] = eulerAngles[2];
angl_midl[0] = Port_1.currentReceivedData.EulerAngle_2.f32;
/******************/
}
#endif
label8.Text = Convert.ToString(Port_1.bodyQuaternion[0]);
label9.Text = Convert.ToString(Port_1.bodyQuaternion[1]);
label10.Text = Convert.ToString(Port_1.bodyQuaternion[2]);
label11.Text = Convert.ToString(Port_1.bodyQuaternion[3]);
label20.Text = Convert.ToString(Port_1.currentReceivedData.Quaternion_0.f32);
label21.Text = Convert.ToString(Port_1.currentReceivedData.Quaternion_1.f32);
label22.Text = Convert.ToString(Port_1.currentReceivedData.Quaternion_2.f32);
label23.Text = Convert.ToString(Port_1.currentReceivedData.Quaternion_3.f32);
qt.Quaternion[0] = Port_1.currentReceivedData.Quaternion_0.f32;
qt.Quaternion[1] = Port_1.currentReceivedData.Quaternion_1.f32;
qt.Quaternion[2] = Port_1.currentReceivedData.Quaternion_2.f32;
qt.Quaternion[3] = Port_1.currentReceivedData.Quaternion_3.f32;
//Save magnetometer data to file for using in calibration algoritm.
string text = Convert.ToString(((float)Port_1.currentReceivedData.Magnetometer.Axis_X.i16)) + "\t" +
Convert.ToString(((float)Port_1.currentReceivedData.Magnetometer.Axis_Y.i16)) + "\t" +
Convert.ToString(((float)Port_1.currentReceivedData.Magnetometer.Axis_Z.i16));
file_magnetometer.WriteLine(text);
/*
* This is test for rotation by rotation matrix
*/
float alpha_rad = 0.03f / 57.2957795130823f;
float[] rot_z = new float[]
{
(float)Math.Cos(alpha_rad) , (float)-Math.Sin(alpha_rad), 0,
(float)Math.Sin(alpha_rad) ,(float) Math.Cos(alpha_rad) , 0,
0 , 0 , 1
};
float[] rot_x = new float[]
{
1,0,0,
0,(float)Math.Cos(alpha_rad) , (float)-Math.Sin(alpha_rad),
0,(float)Math.Sin(alpha_rad) ,(float) Math.Cos(alpha_rad)
};
Matrix.rotMatrix G = new Matrix.rotMatrix();
//rotate body by rotation matrix.
G.MatrixMultiplication(rot_z, Form_3D_DA.RotationMatrix);
//G.MatrixMultiplication(rot_x, G.A3x3);
//Rotate body by rotation matrix.
/// Form_3D_DA.RotationMatrix = G.A3x3;
/*
* End of test
*/
x_IMU_API.QuaternionData q = new x_IMU_API.QuaternionData();
q.Quaternion =new float[] {
(float)Math.Cos(alpha_rad/2.0),
(float)Math.Sin(alpha_rad/2.0)*1.0f, //Z
(float)Math.Sin(alpha_rad/2.0)*0.0f, //Y
(float)Math.Sin(alpha_rad/2.0)*0.0f //Z
};
//Rotate body by quaternion.
// qw.QuaternionMultiplication(q.Quaternion);
alpha_rad = -0.02f / 57.2957795130823f;
q.Quaternion[0] = (float)Math.Cos(alpha_rad / 2.0);
q.Quaternion[1] = (float)Math.Sin(alpha_rad / 2.0) * 0.0f; //Z
q.Quaternion[2] = (float)Math.Sin(alpha_rad / 2.0) * 1.0f; //Y
q.Quaternion[3] = (float)Math.Sin(alpha_rad / 2.0) * 0.0f; //Z
// qw.QuaternionMultiplication(q.Quaternion);
Form_3D_DA.RotationMatrix = qw.ConvertToConjugate().ConvertToRotationMatrix();
// Form_3D_DA.RotationMatrix = qw.ConvertToConjugate().ConvertToRotationMatrix();
Form_3D_DB.RotationMatrix = qt.ConvertToConjugate().ConvertToRotationMatrix();
#if DEMO
if (Graph_2D_DA != null)
{
Graph_2D_DA.DrawGraph(
(double)alpha,
(double)a,
0,
1000.0,
200,
zGMaxAmountOfPoints
);
Graph_2D_DA.DrawGraph(
(double)alpha,
(double)b,
1,
1000.0,
200,
zGMaxAmountOfPoints
);
}
if (Graph_2D_DB != null)
{
Graph_2D_DB.DrawGraph(
(double)alpha,
(double)b,
0,
1000.0,
200,
zGMaxAmountOfPoints
);
Graph_2D_DB.DrawGraph(
(double)alpha+10.3,
(double)b,
1,
1000.0,
200,
zGMaxAmountOfPoints
);
}
#endif
alpha = alpha + 0.05f;
// label2.Text = Convert.ToString(alpha);
}
private void button1_Click(object sender, EventArgs e)
{
//Port_1.MagnetAzimutCalculation(0, 0, -1, -1, 0, 3);
x_IMU_API.QuaternionData qtest = new x_IMU_API.QuaternionData();
qtest.Quaternion = new float [] {1f,0f,0f,0f};
float alpha_degree = 10;
float alpha_rad = alpha_degree / 57.2957795130823f;
float[] q = new float[] {
(float)Math.Cos(alpha_rad / 2.0),
(float)Math.Sin(alpha_rad / 2.0),
(float)Math.Sin(alpha_rad / 2.0),
(float)Math.Sin(alpha_rad / 2.0)
};
qtest.QuaternionMultiplication(q);
}
/// <summary>
/// Quaternion data received event to update rolling ball orientation data.
/// </summary>
static void xIMUserial_QuaternionDataReceived(object sender, x_IMU_API.QuaternionData e)
{
ballTracking.RotationMatrix = e.ConvertToRotationMatrix();
}
