/* ==========================================================================
* == ==
* == FUNCTION KevinFilter ==
* == ==
* == DESCRIPTION A moving weighted average filter ==
* == ==
* == ENTRY int[] filtin - the unfiltered input array ==
* == double[] gauss - the filter window ==
* == double[] filtout - the filtered output array ==
* == out double average - the average value in the output array ==
* == bool db - a flag for conversion to dB scale ==
* == ==
* == RETURN the maximum value in the input array ==
* ========================================================================== */
public static double FindStrength(double[] filtin, MRM_SCAN_INFO t, Point loc, Point radar, double offset, double maxstren, double inmax, GraphParams gp)
{
double coeff = (double)gp.CmWidth / (double)gp.PixWidth;
int radX = (gp.PixWidth * radar.X) / gp.CmWidth;
int radY = (gp.PixHeight * radar.Y) / gp.CmHeight;
double bitDist = PSPERCM * (coeff * Math.Sqrt((loc.X - radX) * (loc.X - radX) + (loc.Y - radY) * (loc.Y - radY)) - offset); //Pixel's distance from radar 1 in picoseconds
if (filtin != null)
{
int dist = (int)((bitDist - (double)t.ScanStartPS + 12000.0) / (((double)t.ScanStepBins) * PSPERBIN)); //converts picoseconds to range bin index
if (dist >= filtin.Length)
{
dist = filtin.Length - 1;
}
if (dist < 0)
{
dist = 0;
}
return((maxstren / inmax) * filtin[dist]); // *(filtin[dist] / inmax);
}
else
{
return(1);
}
}
/* ==========================================================================
* == ==
* == FUNCTION MotionFilter ==
* == ==
* == DESCRIPTION A 4-tap motion filter for a ScanData array ==
* == ==
* == ENTRY ref Radar r - contains previous unfiltered scans ==
* == ref MRM_SCAN_INFO temp - current unfiltered scan
* == ==
* == RETURN none ==
* ========================================================================== */
public static void MotionFilter(ref Radar r, ref MRM_SCAN_INFO temp)
{
for (int k = r.UnfilteredScans.Length - 1; k > 0; k--)
{
r.UnfilteredScans[k] = r.UnfilteredScans[k - 1];
}
r.UnfilteredScans[0] = temp;
r.UnfilteredScans[0].ScanData = new int[temp.ScanData.Length];
temp.ScanData.CopyTo(r.UnfilteredScans[0].ScanData, 0);
for (int i = 1; i < r.UnfilteredScans.Length; i++)
{
if (r.UnfilteredScans[i].ScanData != null)
{
for (int j = 0; j < r.UnfilteredScans[i].ScanData.Length; j++)
//int[] tarray = new int[temp.ScanData.Length];
//for(int j = 3; j < temp.ScanData.Length; j++)
{
if (temp.ScanData.Length > j)
{
temp.ScanData[j] += (int)(r.FilterCoeffs[i] * r.UnfilteredScans[i].ScanData[j]);
}
//for(int i = 0; i<r.FilterCoeffs.Length; i++)
// tarray[j] += (int)(r.FilterCoeffs[i] * temp.ScanData[j - i]);
}
}
//temp.ScanData = tarray;
}
}
public static bool ReceiveMRM_SCAN_INFO(string b, out MRM_SCAN_INFO t)
{
try
{
byte[] s = Encoding.Default.GetBytes(b);
t.MessageID = Utility.ToUshort(s, 6);
t.SourceID = Utility.ToUint(s, 8);
t.Timestamp = Utility.ToUint(s, 12);
t.Reserved1 = Utility.ToUint(s, 16);
t.Reserved2 = Utility.ToUint(s, 20);
t.Reserved3 = Utility.ToUint(s, 24);
t.Reserved4 = Utility.ToUint(s, 28);
t.ScanStartPS = Utility.ToInt(s, 32);
t.ScanStopPS = Utility.ToInt(s, 36);
t.ScanStepBins = Utility.ToShort(s, 40);
t.ScanType = (byte)s[42];
t.Reserved5 = (byte)s[43];
t.AntennaID = (byte)s[44];
t.OperationalMode = (byte)s[45];
t.NumberOfSamplesInMessage = Utility.ToUshort(s, 46);
t.NumberOfSamplesTotal = Utility.ToUint(s, 48);
t.MessageIndex = Utility.ToUshort(s, 52);
t.NumberOfMessagesTotal = Utility.ToUshort(s, 54);
t.ScanData = new int[(s.Length - 56) / 4];
for (int a = 56; a <= s.Length - 4; a += 4)
{
t.ScanData[(a - 56) / 4] = Utility.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 int[350];
return(false);
}
}
public Radar(Socket h, string name, int x, int y)
{
Location = new Point(x, y);
Name = name;
Offset = 0.0;
Scans = new List <MRM_SCAN_INFO>();
CurrentScan = new MRM_SCAN_INFO();
UnfilteredScans = new MRM_SCAN_INFO[FilterCoeffs.Length];
_handler = h;
MRM_GET_STATUSINFO_REQUEST t = new MRM_GET_STATUSINFO_REQUEST();
t.MessageID = 0;
if (_handler != null)
{
RadarRequest.SendMRM_GET_STATUSINFO_REQUEST(this, t);
}
}
public static bool SendMRM_SCAN_INFO(out byte[] b, MRM_SCAN_INFO t)
{
try
{
b = new byte[54 + 4 * t.ScanData.Length];
Utility.FromUshort(0xA5A5, b, 0);
Utility.FromUshort((ushort)(b.Length - 4), b, 2);
Utility.FromUshort(t.MessageID, b, 4);
Utility.FromUint(t.SourceID, b, 6);
Utility.FromUint(t.Timestamp, b, 10);
Utility.FromUint(t.Reserved1, b, 14);
Utility.FromUint(t.Reserved2, b, 18);
Utility.FromUint(t.Reserved3, b, 22);
Utility.FromUint(t.Reserved4, b, 26);
Utility.FromInt(t.ScanStartPS, b, 30);
Utility.FromInt(t.ScanStopPS, b, 34);
Utility.FromShort(t.ScanStepBins, b, 38);
b[40] = t.ScanType;
b[41] = t.Reserved5;
b[42] = t.AntennaID;
b[43] = t.OperationalMode;
Utility.FromUshort(t.NumberOfSamplesInMessage, b, 44);
Utility.FromUint(t.NumberOfSamplesTotal, b, 46);
Utility.FromUshort(t.MessageIndex, b, 50);
Utility.FromUshort(t.NumberOfMessagesTotal, b, 52);
for (int k = 54; k < b.Length - 3; k += 4)
{
Utility.FromInt(t.ScanData[(k - 54) / 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;
int index = 0;
string msg;
bool initialized = false;
n.Scans.Clear();
while ((msg = tr.ReadLine()) != null)
{
string tempVal = "";
int a = 0, b = 0;
maxScanMag = 1;
index = 0;
tempScan = new MRM_SCAN_INFO();
tempScan.ScanData = new int[1750];
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[index] = scanMag;
index++;
}
catch (System.FormatException)
{
entry = DataType.None;
}
break;
}
a = b + 1;
}
entry = DataType.Timestamp;
if (initialized)
{
Filter.BandpassFilter(ref tempScan.ScanData);
Filter.MotionFilter(ref n, ref tempScan);
n.Scans.Add(tempScan);
initialized = false;
index = 0;
}
}
}
/* ==========================================================================
* == ==
* == FUNCTION DrawGraph ==
* == ==
* == DESCRIPTION Draw the 2-D backprojection plot ==
* == ==
* == ENTRY Bitmap bit - the Bitmap object to draw on ==
* == List<Radar> - a List of the Radars to process ==
* == int cmWidth - the width of the enclosure in cm ==
* == GraphParams gp - all GUI parameters needed ==
* == ==
* == RETURN A bool representing whether the operation completed successfully ==
* ========================================================================== */
public static bool DrawGraph(ref System.Drawing.Bitmap bit, List <Radar> Radars, double[] gauss, GraphParams gp)
{
if (Radars.Count == 0)
{
return(false);
}
double[][] filters = new double[Radars.Count][];
int[][] peaks = new int[Radars.Count][];
double[] maxVals = new double[Radars.Count];
double totalStrength = 1.0;
double strengthDivisor;
double maxStrength = 1.0;
byte intensity;
byte count;
try
{
for (int z = 0; z < Radars.Count; z++) //Filter all radars' current ScanData field
{
if (Radars[z].CurrentScan.ScanData != null)
{
int[] temp;
lock (Program.lockObj)
{
temp = new int[Radars[z].CurrentScan.ScanData.Length];
Radars[z].CurrentScan.ScanData.CopyTo(temp, 0); //temp now references a different array, not an alias to CurrentScan.ScanData
MRM_SCAN_INFO ts = Radars[z].CurrentScan;
}
filters[z] = Filter.FilterData(temp, gauss, out peaks[z], gp.DB);
maxVals[z] = filters[z].Max();
}
}
maxStrength = maxVals.Max();
for (int j = 0; j < gp.PixHeight; j++) //Fill the Bitmap pixel by pixel
//Could perhaps be offloaded to GPU in future for potentially faster drawing time
//300x300 Bitmap means 90000 independent calculations... okay to be done on CPU
//1000x1000 Bitmap (perhaps more desirable) means 1000000 independent calculations... unfeasible task for CPU
{
for (int i = 0; i < gp.PixWidth; i++)
{
count = 0;
strengthDivisor = 1.0;
totalStrength = 1.0;
bool IsZero = false;
for (int z = 0; z < Radars.Count; z++) //Find the strength of each radar signal at the range bin corresponding to this pixel
//This automatically provides the radial plot we need by just knowing the distance between the pixel and the radar in terms of range bins
{
if (filters[z] != null)
{
count++;
double stren = Filter.FindStrength(filters[z], Radars[z].CurrentScan, new Point(i, j), Radars[z].Location, Radars[z].Offset, maxStrength, maxVals[z], gp);
if (stren <= 0)
{
IsZero = true;
}
if ((gp.Filtering % 2) == 1)
{
stren = (stren * stren) / maxStrength;
}
if (gp.Multiply)
{
totalStrength *= stren;
}
else
{
totalStrength += stren;
}
}
}
if (IsZero)
{
totalStrength = 0;
}
else
{
totalStrength = Math.Pow(totalStrength, gp.Gamma); //Boosting function: (sum or product of all radar strengths)^Gamma. Provides greater flexibility than a simple add or multiply function
}
for (int k = 0; k < count; k++)
{
if (gp.Multiply)
{
strengthDivisor *= maxStrength;
}
else
{
strengthDivisor += maxStrength;
}
}
strengthDivisor = Math.Pow(strengthDivisor, gp.Gamma);
if (gp.Filtering >= 2)
{
totalStrength = (totalStrength * totalStrength) / strengthDivisor;
}
if (gp.Color) //Scale for color mode
{
double quotient = totalStrength / strengthDivisor;
if (quotient > 1.0)
{
quotient = 1.0;
}
byte red = (quotient > 0.5 ? (byte)(255 * ((quotient - 0.5) / 0.5)) : (byte)0);
byte green = (quotient > 0.5 ? (byte)(255 - 255 * ((quotient - 0.5) / 0.5)) : (byte)(255 * (quotient / 0.5)));
byte blue = (quotient < 0.5 ? (byte)(255 - 255 * (quotient / 0.5)) : (byte)0);
bit.SetPixel(i, j, System.Drawing.Color.FromArgb(red, green, blue));
}
else
{
if (totalStrength > strengthDivisor)
{
count++;
}
intensity = (byte)((255 * totalStrength) / strengthDivisor);
bit.SetPixel(i, j, System.Drawing.Color.FromArgb(intensity, intensity, intensity));
}
}
}
#if TIMER
// if (!radarGraph.IsDisposed && radarGraph.Visible)
// radarGraph.Draw(filters, maxVals, peaks);
#endif
return(true);
}
catch
{
return(false);
}
}
