private void radar_ScanReceived(object sender, MRM_SCAN_INFO ts)
{
byte[] b;
if (RadarRequest.SendMRM_SCAN_INFO(out b, ts))
{
_async.Send(b);
}
//string s = "Scan " + m.MessageID.ToString() + " of length " + m.ScanData.Count.ToString() + " was received";
//_async.Send(Encoding.Default.GetBytes(s));
try
{
if (wr != null)
{
wr.Write(System.DateTime.Now.Ticks + ", MrmFullScanInfo, " + ts.MessageID + ", " + ts.SourceID + ", " + ts.Timestamp + ", ");
wr.Write(ts.Reserved1 + ", " + ts.Reserved2 + ", " + ts.Reserved3 + ", " + ts.Reserved4 + ", " + ts.ScanStartPS + ", " + ts.ScanStopPS + ", " + ts.ScanStepBins + ", ");
wr.Write("1, " + ts.AntennaID + ", " + ts.Reserved5 + ", " + ts.NumberOfSamplesTotal);
foreach (int az in ts.ScanData)
{
wr.Write(", " + az);
}
wr.Write('\n');
}
}
catch (ObjectDisposedException e)
{
System.Diagnostics.Debug.WriteLine(e);
}
}
public static bool ReceiveMRM_SCAN_INFO(string b, out MRM_SCAN_INFO t)
{
try
{
byte[] s = Encoding.Default.GetBytes(b);
t.MessageID = ToUshort(s, 0);
t.SourceID = ToUint(s, 2);
t.Timestamp = ToUint(s, 6);
t.Reserved1 = ToUint(s, 10);
t.Reserved2 = ToUint(s, 14);
t.Reserved3 = ToUint(s, 18);
t.Reserved4 = ToUint(s, 22);
t.ScanStartPS = ToInt(s, 26);
t.ScanStopPS = ToInt(s, 30);
t.ScanStepBins = ToShort(s, 34);
t.ScanType = (byte)s[36];
t.Reserved5 = (byte)s[37];
t.AntennaID = (byte)s[38];
t.OperationalMode = (byte)s[39];
t.NumberOfSamplesInMessage = ToUshort(s, 40);
t.NumberOfSamplesTotal = ToUint(s, 42);
t.MessageIndex = ToUshort(s, 46);
t.NumberOfMessagesTotal = ToUshort(s, 48);
t.ScanData = new List <int>();
for (int a = 50; a <= s.Length - 4; a += 4)
{
t.ScanData.Add(ToInt(s, a));
}
return(true);
}
catch
{
t.MessageID = 0;
t.SourceID = 0;
t.Timestamp = 0;
t.Reserved1 = 0;
t.Reserved2 = 0;
t.Reserved3 = 0;
t.Reserved4 = 0;
t.ScanStartPS = 0;
t.ScanStopPS = 0;
t.ScanStepBins = 0;
t.ScanType = 0;
t.Reserved5 = 0;
t.AntennaID = 0;
t.OperationalMode = 0;
t.NumberOfSamplesInMessage = 0;
t.NumberOfSamplesTotal = 0;
t.MessageIndex = 0;
t.NumberOfMessagesTotal = 0;
t.ScanData = new List <int>();
return(false);
}
}
public static bool SendMRM_SCAN_INFO(out byte[] b, MRM_SCAN_INFO t)
{
try
{
b = new byte[56 + 4 * t.ScanData.Count];
FromUshort(0xA5A5, b, 0);
FromUshort((ushort)(b.Length - 4), b, 2);
FromUshort(0xF201, b, 4);
FromUshort(t.MessageID, b, 6);
FromUint(t.SourceID, b, 8);
FromUint(t.Timestamp, b, 12);
FromUint(t.Reserved1, b, 16);
FromUint(t.Reserved2, b, 20);
FromUint(t.Reserved3, b, 24);
FromUint(t.Reserved4, b, 28);
FromInt(t.ScanStartPS, b, 32);
FromInt(t.ScanStopPS, b, 36);
FromShort(t.ScanStepBins, b, 40);
b[42] = t.ScanType;
b[43] = t.Reserved5;
b[44] = t.AntennaID;
b[45] = t.OperationalMode;
FromUshort(t.NumberOfSamplesInMessage, b, 46);
FromUint(t.NumberOfSamplesTotal, b, 48);
FromUshort(t.MessageIndex, b, 52);
FromUshort(t.NumberOfMessagesTotal, b, 54);
for (int k = 56; k < b.Length - 3; k += 4)
{
FromInt(t.ScanData[(k - 56) / 4], b, k);
}
return(true);
}
catch
{
b = null;
return(false);
}
}
//Reads a CSV file bound to System.IO.StreamReader tr, and stores the data into Radar object n.
public static void ReadDataFromFile(System.IO.StreamReader tr, Radar n)
{
DataType entry = DataType.Timestamp;
MRM_SCAN_INFO tempScan;
int[,] prev = new int[2, 1750];
ushort msgId = 0;
uint sourceId = 0;
uint timestamp = 0;
int scanStartPs = 0, scanStopPs = 0;
short scanStepBins = 0;
byte scanFiltering = 0, antennaId = 0;
ushort numSamples = 0;
int scanMag = 0, maxScanMag;
string msg;
bool initialized = false;
n.Scans.Clear();
while ((msg = tr.ReadLine()) != null)
{
string tempVal = "";
int a = 0, b = 0;
maxScanMag = 1;
tempScan = new MRM_SCAN_INFO();
tempScan.ScanData = new List <int>();
while (a < msg.Length)
{
b = a;
while (b < msg.Length && msg[b] != ',')
{
b++;
}
if (b >= msg.Length - 1 || (msg[b + 1] == ',' && msg[b - 1] == ','))
{
break;
}
tempVal = msg.Substring(a, b - a);
switch (entry)
{
case DataType.None:
break;
case DataType.Timestamp:
try
{
timestamp = (uint)double.Parse(tempVal);
entry = DataType.Mode;
}
catch (System.FormatException)
{
entry = DataType.None;
}
break;
case DataType.Mode:
if (tempVal == " MrmFullScanInfo")
{
entry = DataType.MessageId;
}
else
{
entry = DataType.None;
}
// else if (tempVal == " Config")
// entry = DataType.NodeId;
break;
/* case DataType.NodeId:
* if (tempVal == " NodeId")
* {
* entry = DataType.None;
* break;
* }
* else entry++;
* break;
* case DataType.ScanStartPs:
* break;
* case DataType.ScanStopPs:
* break;
* case DataType.ScanResolutionBins:
* break;
* case DataType.BaseIntegrationIndex:
* break;
* case DataType.Segment1NumSamples:
* break;
* case DataType.Segment2NumSamples:
* break;
* case DataType.Segment3NumSamples:
* break;
* case DataType.Segment4NumSamples:
* break;
* case DataType.Segment1AdditionalIntegration:
* break;
* case DataType.Segment2AdditionalIntegration:
* break;
* case DataType.Segment3AdditionalIntegration:
* break;
* case DataType.Segment4AdditionalIntegration:
* break;
* case DataType.AntennaMode:
* break;
* case DataType.TransmitGain:
* break;
* case DataType.CodeChannel:
* break;*/
case DataType.MessageId:
if (tempVal == " MessageId")
{
entry = DataType.None;
break;
}
else
{
try
{
msgId = ushort.Parse(tempVal);
entry = DataType.SourceId;
}
catch (System.FormatException)
{
entry = DataType.None;
}
}
break;
case DataType.SourceId:
try
{
sourceId = uint.Parse(tempVal);
entry = DataType.EmbeddedTimestamp;
}
catch (System.FormatException)
{
entry = DataType.None;
}
break;
case DataType.EmbeddedTimestamp:
case DataType.Reserved1:
case DataType.Reserved2:
case DataType.Reserved3:
case DataType.Reserved4:
case DataType.Reserved5:
entry++;
break;
case DataType.StartPs:
try
{
scanStartPs = int.Parse(tempVal);
entry = DataType.StopPs;
}
catch (System.FormatException)
{
entry = DataType.None;
}
break;
case DataType.StopPs:
try
{
scanStopPs = int.Parse(tempVal);
entry = DataType.ScanStepBins;
}
catch (System.FormatException)
{
entry = DataType.None;
}
break;
case DataType.ScanStepBins:
try
{
scanStepBins = short.Parse(tempVal);
entry = DataType.Filtering;
}
catch (System.FormatException)
{
entry = DataType.None;
}
break;
case DataType.Filtering:
try
{
scanFiltering = byte.Parse(tempVal);
if (scanFiltering == 1)
{
entry = DataType.AntennaId;
}
else
{
entry = DataType.None;
}
}
catch (System.FormatException)
{
entry = DataType.None;
}
break;
case DataType.AntennaId:
try
{
antennaId = byte.Parse(tempVal);
entry = DataType.Reserved5;
}
catch (System.FormatException)
{
entry = DataType.None;
}
break;
case DataType.NumSamples:
try
{
numSamples = ushort.Parse(tempVal);
tempScan.NumberOfSamplesInMessage = numSamples;
tempScan.MessageID = msgId;
tempScan.SourceID = sourceId;
tempScan.Timestamp = timestamp;
tempScan.Reserved1 = tempScan.Reserved2 = tempScan.Reserved3 = tempScan.Reserved4 = 0;
tempScan.ScanStartPS = scanStartPs;
tempScan.ScanStopPS = scanStopPs;
tempScan.ScanStepBins = scanStepBins;
tempScan.ScanType = scanFiltering;
tempScan.Reserved5 = 0;
tempScan.AntennaID = antennaId;
tempScan.OperationalMode = 0;
tempScan.NumberOfSamplesTotal = 0;
tempScan.MessageIndex = 0;
tempScan.NumberOfMessagesTotal = 0;
initialized = true;
entry = DataType.ScanData;
}
catch (System.FormatException)
{
entry = DataType.None;
}
break;
case DataType.ScanData:
try
{
// int threshold = 1000000;
scanMag = int.Parse(tempVal);
//scanMag = (Math.Abs(tempScan.scanStartPs + index * PSPERBIN * tempScan.scanStepBins) < 30000 ? (scanMag > threshold ? threshold : scanMag) : scanMag);
#if MOTION
prev[1, index] = prev[0, index];
//prev[1,index] = prev[2,index];
//prev[2,index] = prev[3,index];
//prev[3, index] = prev[4, index];
//prev[4, index] = prev[5, index];
//prev[5, index] = prev[6, index];
//prev[6, index] = prev[7, index];
//prev[7, index] = prev[8, index];
prev[0, index] = scanMag;
scanMag = prev[0, index] - prev[1, index];
#endif
if (Math.Abs(scanMag) > Math.Abs(maxScanMag))
{
maxScanMag = scanMag;
}
tempScan.ScanData.Add(scanMag);
}
catch (System.FormatException)
{
entry = DataType.None;
}
break;
}
a = b + 1;
}
entry = DataType.Timestamp;
if (initialized)
{
n.Scans.Add(tempScan);
initialized = false;
}
}
}
private void readPort()
{
try
{
MRM_SCAN_INFO v = new MRM_SCAN_INFO();
v.ScanData = new List <int>();
while (KeepReading)
{
while (KeepReading && _port.BytesToRead < 4)
{
;
}
byte[] s = new byte[4];
int k = _port.Read(s, 0, 4);
if (k == 4)
{
int numBytes = 0;
if (s[0] == 0xA5 && s[1] == 0xA5)
{
numBytes = 256 * s[2] + s[3];
while (KeepReading && _port.BytesToRead < numBytes)
{
;
}
byte[] r = new byte[numBytes];
k = _port.Read(r, 0, numBytes);
ushort code = (ushort)((r[0] << 8) + r[1]);
if (code == 0xF201)
{
MRM_SCAN_INFO t;
if (RadarRequest.ReceiveMRM_SCAN_INFO(Encoding.Default.GetString(r, 2, r.Length - 2), out t))
{
System.Diagnostics.Debug.WriteLine(t.MessageID + " " + t.MessageIndex);
foreach (int i in t.ScanData)
{
v.ScanData.Add(i);
}
if (t.MessageIndex == t.NumberOfMessagesTotal - 1)
{
v.MessageID = t.MessageID;
v.SourceID = t.SourceID;
v.Timestamp = t.Timestamp;
v.Reserved1 = t.Reserved1;
v.Reserved2 = t.Reserved2;
v.Reserved3 = t.Reserved3;
v.Reserved4 = t.Reserved4;
v.ScanStartPS = t.ScanStartPS;
v.ScanStopPS = t.ScanStopPS;
v.ScanStepBins = t.ScanStepBins;
v.ScanType = t.ScanType;
v.Reserved5 = t.Reserved5;
v.AntennaID = t.AntennaID;
v.OperationalMode = t.OperationalMode;
v.NumberOfSamplesInMessage = t.NumberOfSamplesInMessage;
v.NumberOfSamplesTotal = t.NumberOfSamplesTotal;
v.MessageIndex = t.MessageIndex;
v.NumberOfMessagesTotal = t.NumberOfMessagesTotal;
//string path = "C:\\Users\\Dustin\\Radar Data\\Radar Scans\\test.csv";
//if (!System.IO.File.Exists(path)) System.IO.File.Create(path);
//using (System.IO.StreamWriter sw = new System.IO.StreamWriter(path, true))
//{
// sw.Write(v.Timestamp + ", MrmFullScanInfo, " + v.MessageID + ", " + v.SourceID + ", " + v.Timestamp + ", ");
// sw.Write(v.Reserved1 + ", " + v.Reserved2 + ", " + v.Reserved3 + ", " + v.Reserved4 + ", " + v.ScanStartPS + ", " + v.ScanStopPS + ", " + v.ScanStepBins + ", ");
// sw.Write("1, " + v.AntennaID + ", " + v.Reserved5 + ", " + v.NumberOfSamplesTotal);
// foreach (int a in v.ScanData)
// sw.Write(", " + a);
// sw.Write('\n');
// sw.Close();
//}
if (ScanReceived != null)
{
ScanReceived(this, v);
}
v = new MRM_SCAN_INFO();
v.ScanData = new List <int>();
}
}
}
else
{
if (this.CommandReceived != null)
{
this.CommandReceived(this, Encoding.Default.GetString(r));
}
}
}
}
}
}
catch
{
}
}