public void addRangeReading(int angleRaw, double rangeMeters, long timestamp)
{
lock (this)
{
RangeReading rr = new RangeReading(angleRaw, rangeMeters, timestamp);
rangeReadings.Add(rr);
purge();
}
}
private void OnMeasurement(Packet p, DateTime TimeStamp)
{
//Tracer.Trace("CommLink::OnMeasurement()");
int packetLength = p.angles.Count; // typically 26 packets, each covering 7 degrees
if (packetLength != 26)
{
Tracer.Error("CommLink::OnMeasurement() not a standard measurement, angles.Count=" + packetLength + " (expected 26) -- ignored");
return;
}
LinkMeasurement lsd = new LinkMeasurement();
lsd.TimeStamp = TimeStamp;
lsd.Units = Units.Millimeters;
int angularRange = 180;
int mesLength = angularRange + 1; // 181
lsd.Ranges = new int[mesLength]; // millimeters, with 1 degree resolution
List <double> ranges = new List <double>();
foreach (int angle in p.angles.Keys)
{
RangeReading rr = p.getLatestReadingAt(angle);
double range = rr.rangeMeters * 1000.0d; // millimeters
ranges.Add(range);
//Tracer.Trace("&&&&&&&&&&&&&&&&&&&&&&&&&&&& angle=" + angle + " range=" + range);
/*
* typical measurement:
* PACKET READY -- angles: 26 packets: 1
* angle=150 range=1440
* angle=190 range=1450
* angle=230 range=1450
* angle=270 range=1450
* angle=310 range=1460
* angle=350 range=1540
* angle=390 range=1540
* angle=430 range=1700
* angle=470 range=1700
* angle=510 range=1740
* angle=550 range=2260
* angle=590 range=1100
* angle=630 range=1100
* angle=670 range=1090
* angle=710 range=1100
* angle=750 range=1090
* angle=790 range=1090
* angle=830 range=1090
* angle=870 range=1090
* angle=910 range=1700
* angle=950 range=1710
* angle=990 range=1730
* angle=1030 range=1720
* angle=1070 range=1710
* angle=1110 range=3500
* angle=1150 range=3500
*/
}
int step = (int)Math.Round((double)mesLength / (double)packetLength); // typically round(6.96) = 7
// if we smooth the measurements, Decide() has better chance of sorting the values right. It does not like 7 degrees steps.
// we need these for exponential moving average:
double emaPeriod = 4.0d;
double emaMultiplier = 2.0d / (1.0d + emaPeriod);
double?emaPrev = null;
for (int i = 0; i < mesLength; i++)
{
int angleIndex = i / step;
//ushort range = (ushort)(i * 40);
ushort range = (ushort)Math.Round(ranges[angleIndex]);
if (range > 0x1FF7)
{
range = 0x2000;
}
// calculate exponential moving average - smooth the curve a bit:
double?ema = !emaPrev.HasGoodValue() ? range : ((range - emaPrev) * emaMultiplier + emaPrev);
emaPrev = ema;
int iRange = (int)Math.Round((double)ema);
//Tracer.Trace("&&&&&&&&&&&&&&&&&&&&&&&&&&&& i=" + i + " range=" + range + " ema=" + iRange);
//lsd.Ranges[i] = range; // 5000; // milimeters
lsd.Ranges[i] = iRange; // 5000; // milimeters
}
lsd.ScanIndex = -1;
lsd.TelegramIndex = -1;
_internalPort.Post(lsd);
/*
* byte[] data = p.Data;
* LinkMeasurement lsd = new LinkMeasurement();
* lsd.TimeStamp = TimeStamp;
*
* ushort lengthAndFlags = Packet.MakeUshort(data[1], data[2]);
* int length = lengthAndFlags & 0x3FF;
*
* switch (lengthAndFlags >> 14)
* {
* case 0:
* lsd.Units = Units.Centimeters;
* break;
* case 1:
* lsd.Units = Units.Millimeters;
* break;
* default:
* return;
* }
*
* lsd.Ranges = new int[length];
*
* int offset = 3;
* for (int i = 0; i < length; i++, offset += 2)
* {
* ushort range = Packet.MakeUshort(data[offset], data[offset + 1]);
* if (range > 0x1FF7)
* {
* range = 0x2000;
* }
* lsd.Ranges[i] = range;
* }
*
*
* if (offset < p.Length - 1)
* {
* lsd.ScanIndex = data[offset++];
* }
* else
* {
* lsd.ScanIndex = -1;
* }
* if (offset < p.Length - 1)
* {
* lsd.TelegramIndex = data[offset++];
* }
* else
* {
* lsd.TelegramIndex = -1;
* }
*
* _internalPort.Post(lsd);
*/
}
