public MainController(MainWindow x, AttitudeData y)
{
this._main = x;
this._data = y;
this._usb = new USB(x, y, this);
this._usbConnected = false;
this.setDataAddress();
this.refreshComports();
}
void Start()
{
attitudeData = new List <AttitudeData>();
gpsData = new List <GpsData>();
StreamReader attitudeStreamReader = GetFileStream("result.ahr2");
string line = attitudeStreamReader.ReadLine();
string[] dt = line.Split(' ');
while (line != null)
{
AttitudeData d = new AttitudeData {
Timestamp = int.Parse(dt[0]),
Roll = float.Parse(dt[1]),
Pitch = float.Parse(dt[2]),
Yaw = float.Parse(dt[3])
};
attitudeData.Add(d);
line = attitudeStreamReader.ReadLine();
if (line != null)
{
dt = line.Split(' ');
}
}
attitudeStreamReader.Close();
StreamReader gpsStreamReader = GetFileStream("result.gps");
line = gpsStreamReader.ReadLine();
dt = line.Split(' ');
while (line != null)
{
GpsData d = new GpsData {
Timestamp = int.Parse(dt[0]),
Latitude = float.Parse(dt[1]),
Longitude = float.Parse(dt[2]),
Altitude = float.Parse(dt[3])
};
gpsData.Add(d);
line = gpsStreamReader.ReadLine();
if (line != null)
{
dt = line.Split(' ');
}
}
gpsStreamReader.Close();
Vector3 offset = new Vector3(-0.362f, 0.288f, 1.83f);
for (int i = 0; i < gpsData.Count - 1; i++)
{
Debug.DrawLine(new Vector3(gpsData[i].Longitude, gpsData[i].Altitude, gpsData[i].Latitude),
new Vector3(gpsData[i + 1].Longitude, gpsData[i + 1].Altitude, gpsData[i + 1].Latitude), Color.red, 3000.0f);
}
}
public USB(MainWindow x, AttitudeData y, MainController z)
{
sw = new Stopwatch();
this._mainCtrl = z;
this._main = x;
this._data = y;
this._serial = new SerialPort();
this._portsList = SerialPort.GetPortNames();
this._numberOfPorts = this._portsList.Length;
this._collectionMode = false;
this._numberOfDataGathered = 0;
this._parsingRate = 50; //percentage
this._parsingCounter = 0;
//_dataPackage = new DataWrapper();
}
public MainWindow()
{
InitializeComponent();
Color color = new Color();
color = Colors.Azure;
this.Background = new SolidColorBrush(color);
this._data = new AttitudeData();
this._ctrl = new MainController(this, this._data);
this._gyro = new TextBox[10];
this._acc = new TextBox[10];
this._cFilter = new TextBox[10];
this._gyro[0] = this.txtGRawX;
this._gyro[1] = this.txtGRawY;
this._gyro[2] = this.txtGRawZ;
this._gyro[3] = this.txtGNorm;
this._gyro[4] = this.txtGDirCosX;
this._gyro[5] = this.txtGDegreeX;
this._gyro[6] = this.txtGDirCosY;
this._gyro[7] = this.txtGDegreeY;
this._gyro[8] = this.txtGDirCosZ;
this._gyro[9] = this.txtGDegreeZ;
this._acc[0] = this.txtARawX;
this._acc[1] = this.txtARawY;
this._acc[2] = this.txtARawZ;
this._acc[3] = this.txtANorm;
this._acc[4] = this.txtADirCosX;
this._acc[5] = this.txtADegreeX;
this._acc[6] = this.txtADirCosY;
this._acc[7] = this.txtADegreeY;
this._acc[8] = this.txtADirCosZ;
this._acc[9] = this.txtADegreeZ;
this._cFilter[0] = this.txtCRawX;
this._cFilter[1] = this.txtCRawY;
this._cFilter[2] = this.txtCRawZ;
this._cFilter[3] = this.txtCNorm;
this._cFilter[4] = this.txtCDirCosX;
this._cFilter[5] = this.txtCDegreeX;
this._cFilter[6] = this.txtCDirCosY;
this._cFilter[7] = this.txtCDegreeY;
this._cFilter[8] = this.txtCDirCosZ;
this._cFilter[9] = this.txtCDegreeZ;
}
public void BroadcastAttitudeData(AttitudeData attitudeData)
{
string dataString = $"XATTPrepar3D,{attitudeData.trueHeading:F1},{attitudeData.pitch:F1},{attitudeData.roll:F1},0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0";
SendData(dataString);
}
public void saveToExcel()
{
Console.WriteLine("Saving...");
Excel.Application xlFile = new Microsoft.Office.Interop.Excel.Application();
if (xlFile == null)
{
MessageBox.Show("Excel is not properly installed!!");
return;
}
Excel.Workbook xlBook;
Excel.Worksheet[] sheet = new Excel.Worksheet[5];
object misValue = System.Reflection.Missing.Value;
xlBook = xlFile.Workbooks.Add(misValue);
int outterIteration = this._data.gyro.Length;
int innerIteration = this._data.gyro[0].Count;
// sheet one - gyro
sheet[0] = (Excel.Worksheet)xlBook.Worksheets.get_Item(1);
sheet[0].Name = (string)"Gyroscope";
sheet[0].Cells[1, 1] = "RawX";
sheet[0].Cells[1, 2] = "RawY";
sheet[0].Cells[1, 3] = "RawZ";
sheet[0].Cells[1, 4] = "Normal";
sheet[0].Cells[1, 5] = "DircosX";
sheet[0].Cells[1, 6] = "Degreex";
sheet[0].Cells[1, 7] = "DirCosY";
sheet[0].Cells[1, 8] = "DegreeY";
sheet[0].Cells[1, 9] = "DircosZ";
sheet[0].Cells[1, 10] = "DegreeZ";
for (int i = 0; i < outterIteration; i++)
{
List <string> temp = this._data.gyro[i];
for (int j = 0; j < innerIteration; j++)
{
//Console.WriteLine("asfasdf");
//Console.WriteLine(i);
sheet[0].Cells[j + 2, i + 1] = temp[j];
}
}
//Console.WriteLine("2");
// sheet two - acc
sheet[1] = (Excel.Worksheet)xlBook.Worksheets.Add();
sheet[1].Name = (string)"Accelerometer";
sheet[1].Cells[1, 1] = "RawX";
sheet[1].Cells[1, 2] = "RawY";
sheet[1].Cells[1, 3] = "RawZ";
sheet[1].Cells[1, 4] = "Normal";
sheet[1].Cells[1, 5] = "DircosX";
sheet[1].Cells[1, 6] = "Degreex";
sheet[1].Cells[1, 7] = "DirCosY";
sheet[1].Cells[1, 8] = "DegreeY";
sheet[1].Cells[1, 9] = "DircosZ";
sheet[1].Cells[1, 10] = "DegreeZ";
for (int i = 0; i < outterIteration; i++)
{
List <string> temp = this._data.acc[i];
for (int j = 0; j < innerIteration; j++)
{
sheet[1].Cells[j + 2, i + 1] = temp[j];
}
}
// sheet three - cFilter
sheet[2] = (Excel.Worksheet)xlBook.Worksheets.Add();
sheet[2].Name = (string)"cFilter";
sheet[2].Cells[1, 1] = "RawX";
sheet[2].Cells[1, 2] = "RawY";
sheet[2].Cells[1, 3] = "RawZ";
sheet[2].Cells[1, 4] = "Normal";
sheet[2].Cells[1, 5] = "DircosX";
sheet[2].Cells[1, 6] = "Degreex";
sheet[2].Cells[1, 7] = "DirCosY";
sheet[2].Cells[1, 8] = "DegreeY";
sheet[2].Cells[1, 9] = "DircosZ";
sheet[2].Cells[1, 10] = "DegreeZ";
for (int i = 0; i < outterIteration; i++)
{
List <string> temp = this._data.cFilter[i];
for (int j = 0; j < innerIteration; j++)
{
sheet[2].Cells[j + 2, i + 1] = temp[j];
}
}
// sheet four - magnetometer
sheet[3] = (Excel.Worksheet)xlBook.Worksheets.Add();
sheet[3].Name = (string)"Magnetometer";
sheet[3].Cells[1, 1] = "MagX";
sheet[3].Cells[1, 2] = "MagY";
sheet[3].Cells[1, 3] = "MagZ";
for (int i = 0; i < 3; i++)
{
List <string> temp = this._data.mag[i];
for (int j = 0; j < innerIteration; j++)
{
sheet[3].Cells[j + 2, i + 1] = temp[j];
}
}
// sheet fives - control ref
sheet[4] = (Excel.Worksheet)xlBook.Worksheets.Add();
sheet[4].Name = (string)"Attitude Reference";
sheet[4].Cells[1, 1] = "pitch";
sheet[4].Cells[1, 2] = "roll";
sheet[4].Cells[1, 3] = "yaw";
sheet[4].Cells[1, 5] = "Throttle";
for (int i = 0; i < 3; i++)
{
List <string> temp = this._data.ctrlRefAtt[i];
for (int j = 0; j < innerIteration; j++)
{
sheet[4].Cells[j + 2, i + 1] = temp[j];
if (i < 1)
{
List <string> throt = this._data.Throttle;
sheet[4].Cells[j + 2, 5] = throt[j];
}
}
}
xlBook.SaveAs(this._main.lblAddress.Content + this._main.txtFileName.Text);
xlBook.Close(true, misValue, misValue);
xlFile.Quit();
releaseObject(sheet[0]);
releaseObject(sheet[1]);
releaseObject(sheet[2]);
releaseObject(sheet[3]);
releaseObject(sheet[4]);
releaseObject(xlBook);
releaseObject(xlFile);
MessageBox.Show("Excel file created , you can find the file " + this._main.lblAddress.Content + this._main.txtFileName.Text);
this._data = new AttitudeData();
}
/// <summary>
/// Initializes a new instance of the PacketSniffer class that contain the values extracted from the given byte array.
/// </summary>
/// <param name="byteArray">14 bytes array for basic information OR 256 byte for packet time.</param>
public PacketSniffer(Byte[] byteArray)
{
_MagicNumber = 0;
_HeaderType = 0;
_SubChannelNumber = 0;
_NumberChannelsToFollow = 0;
_NumberBytesThisRecord = 0;
_PacketTime = DateTime.MinValue;
_TimeTag = 0;
using (BinaryReader pd = new BinaryReader(ArrayToStream.BytesToMemory(byteArray)))
{
if (byteArray.Length >= 14)
{
// Read the basic information of the packet
XtfBasePacket basePkt = new XtfBasePacket(byteArray);
_MagicNumber = basePkt.MagicNumber;
_HeaderType = basePkt.HeaderType;
_SubChannelNumber = basePkt.SubChannelNumber;
_NumberChannelsToFollow = basePkt.NumberChannelsToFollow;
_NumberBytesThisRecord = basePkt.NumberBytesThisRecord;
if (byteArray.Length >= 256)
{
// Extract packet time from each packet type known
switch (HeaderType)
{
// Ping header packet types
case 0:
case 2:
case 4:
case 5:
case 8:
case 9:
case 10:
case 14:
case 16:
case 17:
case 18:
case 19:
case 22:
case 25:
case 27:
case 28:
case 60:
case 61:
case 62:
case 65:
case 66:
case 68:
case 69:
case 73:
PingHeader tmpHdr0 = new PingHeader(byteArray);
_PacketTime = tmpHdr0.PacketTime;
break;
// Notes - text annotation packet types
case 1:
Notes tmpHdr1 = new Notes(byteArray);
_PacketTime = tmpHdr1.PacketTime;
break;
// Attitude Data packet types
case 3:
case 103:
AttitudeData tmpHdr2 = new AttitudeData(byteArray);
_PacketTime = tmpHdr2.PacketTime;
_TimeTag = tmpHdr2.TimeTag;
break;
// Raw Serial packet types
case 6:
RawSerialHeader tmpHdr3 = new RawSerialHeader(byteArray);
_PacketTime = tmpHdr3.PacketTime;
break;
// High speed sensor packet types
case 11:
case 15:
HighSpeedSensor tmpHdr4 = new HighSpeedSensor(byteArray);
_PacketTime = tmpHdr4.PacketTime;
break;
// Gyro packet types
case 23:
case 84:
Gyro tmpHdr5 = new Gyro(byteArray);
_PacketTime = tmpHdr5.PacketTime;
_TimeTag = tmpHdr5.TimeTag;
break;
// QPS Singlebeam packet types
case 26:
QpsSingleBeam tmpHdr6 = new QpsSingleBeam(byteArray);
_PacketTime = tmpHdr6.PacketTime;
break;
// Navigation packet types
case 42:
case 100:
Navigation tmpHdr7 = new Navigation(byteArray);
_PacketTime = tmpHdr7.PacketTime;
_TimeTag = tmpHdr7.TimeTag;
break;
// Pos Raw Navigation packet types
case 107:
PosRawNavigation tmpHdr8 = new PosRawNavigation(byteArray);
_PacketTime = tmpHdr8.PacketTime;
break;
// Navigation packet types
case 199:
RawCustomHeader tmpHdr9 = new RawCustomHeader(byteArray);
_PacketTime = tmpHdr9.PacketTime;
_TimeTag = tmpHdr9.TimeTag;
break;
default:
break;
}
}
}
}
}
public MainWindow()
{
InitializeComponent();
_cellBar = new ProgressBar[6];
_cellInfo = new TextBox[6];
_cellBar[0] = prgLipoC0;
_cellBar[1] = prgLipoC1;
_cellBar[2] = prgLipoC2;
_cellBar[3] = prgLipoC3;
_cellBar[4] = prgLipoC4;
_cellBar[5] = prgLipoC5;
_cellInfo[0] = txtLipoC0;
_cellInfo[1] = txtLipoC1;
_cellInfo[2] = txtLipoC2;
_cellInfo[3] = txtLipoC3;
_cellInfo[4] = txtLipoC4;
_cellInfo[5] = txtLipoC5;
//prgLipoC0.Value = 10;
//prgLipoC0.Foreground = new SolidColorBrush(Colors.Red);
Color color = new Color();
color = Colors.Azure;
this.Background = new SolidColorBrush(color);
this._data = new AttitudeData();
this._ctrl = new MainController(this, this._data);
this._gyro = new TextBox[10];
this._acc = new TextBox[10];
this._cFilter = new TextBox[10];
this._gyro[0] = this.txtGRawX;
this._gyro[1] = this.txtGRawY;
this._gyro[2] = this.txtGRawZ;
this._gyro[3] = this.txtGNorm;
this._gyro[4] = this.txtGDirCosX;
this._gyro[5] = this.txtGDegreeX;
this._gyro[6] = this.txtGDirCosY;
this._gyro[7] = this.txtGDegreeY;
this._gyro[8] = this.txtGDirCosZ;
this._gyro[9] = this.txtGDegreeZ;
this._acc[0] = this.txtARawX;
this._acc[1] = this.txtARawY;
this._acc[2] = this.txtARawZ;
this._acc[3] = this.txtANorm;
this._acc[4] = this.txtADirCosX;
this._acc[5] = this.txtADegreeX;
this._acc[6] = this.txtADirCosY;
this._acc[7] = this.txtADegreeY;
this._acc[8] = this.txtADirCosZ;
this._acc[9] = this.txtADegreeZ;
this._cFilter[0] = this.txtCRawX;
this._cFilter[1] = this.txtCRawY;
this._cFilter[2] = this.txtCRawZ;
this._cFilter[3] = this.txtCNorm;
this._cFilter[4] = this.txtCDirCosX;
this._cFilter[5] = this.txtCDegreeX;
this._cFilter[6] = this.txtCDirCosY;
this._cFilter[7] = this.txtCDegreeY;
this._cFilter[8] = this.txtCDirCosZ;
this._cFilter[9] = this.txtCDegreeZ;
}
