public void interpretMessages()
{
//Interpret buffer
if (RxBuffer.Count > 0)
{
//If received a MSG
if (RxBuffer.Count >= RxBuffer[0])
{
//Convert raw bytes to a struct
int count = RxBuffer[0];
RxBuffer.RemoveAt(0);
CurrRegs = fromBytes(RxBuffer.ToArray());
RxBuffer.RemoveRange(0, count - 1);
// xindex is global double;
xIndex += 0.01;
//Update Graphs
updateSensorGraphs();
updateOrientationGraphs();
//update progress bars
updateRemoteMotors();
//Quad copter model updating
quadcopterModel1.UpdateModel(CurrRegs.roll, CurrRegs.pitch, -1 * CurrRegs.yaw);
}
else
{
//MessageBox.Show("There is Error in updateSerialPort()\n" + "Number of data recieved is 0\n");
}
}
send_requestRegisters();
}
/************************************************************
* **********************************************************/
boardRegisters fromBytes(byte[] arr)
{
boardRegisters str = new boardRegisters();
int size = Marshal.SizeOf(str);
IntPtr ptr = Marshal.AllocHGlobal(size);
Marshal.Copy(arr, 0, ptr, size);
str = (boardRegisters)Marshal.PtrToStructure(ptr, str.GetType());
Marshal.FreeHGlobal(ptr);
return str;
}
private void timer1_Tick(object sender, EventArgs e)
{
//Interpret buffer
if (RxBuffer.Count > 0)
{
if (RxBuffer.Count >= (RxBuffer[0]))
{
int count = RxBuffer[0];
RxBuffer.RemoveAt(0);
CurrRegs = fromBytes(RxBuffer.GetRange(0, count - 1).ToArray());
RxBuffer.RemoveRange(0, count - 1);
double roll_ang = (CurrRegs.roll * 180.0 / Math.PI);
double pitch_ang = (CurrRegs.pitch * 180.0 / Math.PI);
double yaw_ang = (CurrRegs.yaw * 180.0 / Math.PI);
//textBox12.Text = regs.roll.ToString();
textBox1.Text = CurrRegs.accel_x.ToString();
textBox2.Text = CurrRegs.accel_y.ToString();
textBox3.Text = CurrRegs.accel_z.ToString();
textBox13.Text = CurrRegs.accel_x_raw.ToString();
textBox14.Text = CurrRegs.accel_y_raw.ToString();
textBox15.Text = CurrRegs.accel_z_raw.ToString();
textBox16.Text = CurrRegs.gyro_x_raw.ToString();
textBox17.Text = CurrRegs.gyro_y_raw.ToString();
textBox18.Text = CurrRegs.gyro_z_raw.ToString();
textBox32.Text = CurrRegs.gyro_x_raw_avg.ToString();
textBox33.Text = CurrRegs.gyro_y_raw_avg.ToString();
textBox34.Text = CurrRegs.gyro_z_raw_avg.ToString();
textBox29.Text = CurrRegs.accel_x_raw_avg.ToString();
textBox30.Text = CurrRegs.accel_y_raw_avg.ToString();
textBox31.Text = CurrRegs.accel_z_raw_avg.ToString();
textBox44.Text = (2e-3 * CurrRegs.accel_x_raw_avg).ToString();
textBox43.Text = (2e-3 * CurrRegs.accel_y_raw_avg).ToString();
textBox42.Text = (2e-3 * CurrRegs.accel_z_raw_avg).ToString();
textBox4.Text = CurrRegs.gyro_x.ToString();
textBox5.Text = CurrRegs.gyro_y.ToString();
textBox6.Text = CurrRegs.gyro_z.ToString();
textBox7.Text = CurrRegs.mag_x.ToString();
textBox8.Text = CurrRegs.mag_y.ToString();
textBox9.Text = CurrRegs.mag_z.ToString();
textBox12.Text = CurrRegs.roll.ToString();
textBox10.Text = CurrRegs.pitch.ToString();
textBox11.Text = CurrRegs.yaw.ToString();
//textBox24.Text = roll_ang.ToString();
//textBox22.Text = pitch_ang.ToString();
//textBox23.Text = yaw_ang.ToString();
textBox26.Text = CurrRegs.desired_roll.ToString();
textBox27.Text = CurrRegs.desired_pitch.ToString();
textBox28.Text = CurrRegs.desired_yaw.ToString();
textBox39.Text = CurrRegs.desired_throttle.ToString();
textBox35.Text = CurrRegs.motor1.ToString();
textBox36.Text = CurrRegs.motor2.ToString();
textBox37.Text = CurrRegs.motor3.ToString();
textBox38.Text = CurrRegs.motor4.ToString();
textBox41.Text = CurrRegs.v_batt.ToString();
//Charts updating
chart1.Series[0].Points.Add(CurrRegs.roll);
chart2.Series[0].Points.Add(CurrRegs.pitch);
chart3.Series[0].Points.Add(CurrRegs.yaw);
chart1.Series[1].Points.Add(CurrRegs.desired_roll);
chart2.Series[1].Points.Add(CurrRegs.desired_pitch);
chart3.Series[1].Points.Add(CurrRegs.desired_yaw);
data_acclX.Add(CurrRegs.gyro_x_raw);
data_acclY.Add(CurrRegs.gyro_y_raw);
data_acclZ.Add(CurrRegs.gyro_z_raw);
chart4.Series[0].Points.Add(CurrRegs.PID_ROLL_ERROR);
chart5.Series[0].Points.Add(CurrRegs.PID_ROLL_ERROR_SUM);
chart6.Series[0].Points.Add(CurrRegs.PID_ROLL_ERROR_DER);
chart7.Series[0].Points.Add(CurrRegs.PID_Roll_P * CurrRegs.PID_ROLL_ERROR);
chart8.Series[0].Points.Add(CurrRegs.PID_Roll_I * CurrRegs.PID_ROLL_ERROR_SUM);
chart9.Series[0].Points.Add(CurrRegs.PID_Roll_D * CurrRegs.PID_ROLL_ERROR_DER);
chart14.Series[0].Points.Add((CurrRegs.PID_Roll_P * CurrRegs.PID_ROLL_ERROR) +
(CurrRegs.PID_Roll_I * CurrRegs.PID_ROLL_ERROR_SUM) +
(CurrRegs.PID_Roll_D * CurrRegs.PID_ROLL_ERROR_DER));
chart10.Series[0].Points.Add(CurrRegs.motor1);
chart11.Series[0].Points.Add(CurrRegs.motor2);
chart12.Series[0].Points.Add(CurrRegs.motor3);
chart13.Series[0].Points.Add(CurrRegs.motor4);
tbMotor1.Text = CurrRegs.motor1.ToString("0.00#####");
tbMotor2.Text = CurrRegs.motor2.ToString("0.00#####");
tbMotor3.Text = CurrRegs.motor3.ToString("0.00#####");
tbMotor4.Text = CurrRegs.motor4.ToString("0.00#####");
//Quad copter model updating
quadcopterModel1.UpdateModel( CurrRegs.roll, CurrRegs.pitch, CurrRegs.yaw);
}
}
List<byte> buffer2 = new List<byte>
{0x10, 0x11, 0x12, 0x13,
0x14, 0x15, 0x16, 0x17,
0x18, 0x19, 0x1A,
//Gyro
0x20, 0x21, 0x22, 0x23,
0x24, 0x25, 0x26, 0x27,
0x28, 0x29, 0x2A,
//mag
0x30, 0x31, 0x32,
0x33, 0x34, 0x35,
//Roll yaw pitch
0x40, 0x41, 0x42,
//Desired pitch, yaw, roll, throttle
0x44, 0x45, 0x46, 0x47,
//motor values
0x50, 0x51, 0x52, 0x53,
//voltage battery
0x60,
//Filter RollPitchP, RollPitchI, YawP, YawI
0x80, 0x81, 0x82, 0x83,
//MotorPID
//Roll PID, Pitch PID, Yaw PID
0x90, 0x91, 0x92,
0x93, 0x94, 0x95,
0x96, 0x97, 0x98,
0x9A, 0x9B, 0x9C
};
buffer2.Insert(0, 0x02); //Read reg
buffer2.Insert(buffer2.Count, 0xFF); //add End of Transmission byte
buffer2.Insert(0, (byte)(buffer2.Count() + 1));
byte[] buffer = buffer2.ToArray();
port1.Write(buffer, 0, buffer.Length);
}
