private void ProviderDataReceived(object sender, ushort[] data)
{
// check the device state
if (DeviceState == DeviceState.Startup)
{
if (m_detectorType == Detector.Old)
{
if (m_calibrationSamples == null)
{
m_calibrationSamples = new double[data.Length];
}
Parallel.For(0, data.Length, i =>
{
m_calibrationSamples[i] += data[i];
});
m_calibrationCounter++;
if (m_calibrationCounter == NumberOfCalibrationSamples)
{
Parallel.For(0, data.Length, i =>
{
m_calibrationSamples[i] /= m_calibrationCounter;
});
DeviceState = DeviceState.Running;
}
}
else
{
if (!m_calibration.IsValid)
{
m_calibration.AddSample(data);
m_calibrationCounter++;
if (m_calibrationCounter == NumberOfCalibrationSamples)
{
// now, we can calibrate
m_calibration.Calibrate();
if (m_calibration.IsValid)
{
DeviceState = DeviceState.Running;
}
else
{
m_calibration.Reset();
}
}
}
}
}
else if (DeviceState == DeviceState.Running)
{
// preprocess the signal (get baseline and shift)
float[] rawData = data.Select(value => (float)value).ToArray();
ushort[] calibratedData = new ushort[rawData.Length];
if (m_detectorType == Detector.Old)
{
// calibrate
Parallel.For(0, rawData.Length, i =>
{
calibratedData[i] = (ushort)Math.Abs(rawData[i] - m_calibrationSamples[i]);
});
}
else if (m_detectorType == Detector.New)
{
Signal rawSignal = Signal.FromArray(rawData, SampleRate, SampleFormat.Format32BitIeeeFloat);
LowPassFilter lowPassFilter = new LowPassFilter(100.0, SampleRate)
{
Alpha = 0.05f
};
rawSignal = lowPassFilter.Apply(rawSignal);
float[] baseline = rawSignal.ToFloat();
Parallel.For(0, rawData.Length, i =>
{
rawData[i] = Math.Abs(rawData[i] - baseline[i]);
});
}
else if (m_detectorType == Detector.Advanced)
{
}
// good, raw data is shifted now
SensorDataEventArgs eventArgs = null;
if (m_detectorType == Detector.Old)
{
eventArgs = m_signalProcessor.ProcessData(calibratedData, true);
}
else
{
eventArgs = m_signalProcessor.ProcessData(m_calibration, rawData, true);
}
// notify the receviers about the raw data
DataReceived?.Invoke(this, eventArgs);
// now, do touch processing
ProcessTouches(eventArgs);
}
}
private void ProcessTouches(SensorDataEventArgs eventArgs)
{
List <EchoPoint> currPts = new List <EchoPoint>();
// first: calculate the new points
for (int i = 0; i < eventArgs.Peaks.Length; i++)
{
double distance = GetDistance(eventArgs.Peaks[i]);
double pressure = eventArgs.Amplitudes[i] / 1000.0;
pressure = Math.Min(Math.Max(0.0, pressure), 1.0);
EchoPoint echoPt = new EchoPoint(i, distance, pressure,
eventArgs.Peaks[i], eventArgs.Amplitudes[i]);
currPts.Add(echoPt);
}
// match the points
if (m_prevPoints.Count == 0)
{
// we did not have previous points
if (currPts.Count != 0)
{
// match those points to new points
for (int i = 0; i < currPts.Count; i++)
{
m_prevPoints.Add(currPts[i]);
TouchDown?.Invoke(this, new EchoPointEventArgs(currPts[i]));
}
}
else
{
// nothing to do here
}
}
else if (m_prevPoints.Count == 1)
{
// there was one previous point
if (currPts.Count == 0)
{
// the point disappeared
TouchUp?.Invoke(this, new EchoPointEventArgs(m_prevPoints[0]));
m_prevPoints.RemoveAt(0);
}
else if (currPts.Count == 1)
{
// this is the same point (double-check distance)
m_prevPoints[0].Update(currPts[0]);
TouchMove?.Invoke(this, new EchoPointEventArgs(m_prevPoints[0]));
}
else if (currPts.Count == 2)
{
// we added a point
// find out which one was the original
double distance1 = Math.Abs(currPts[0].Distance - m_prevPoints[0].Distance);
double distance2 = Math.Abs(currPts[1].Distance - m_prevPoints[0].Distance);
int correctId = m_prevPoints[0].Id == 0 ? 1 : 0;
if (distance1 > distance2)
{
// the first point is new
currPts[0].Id = correctId;
m_prevPoints[0].Update(currPts[1]);
TouchMove?.Invoke(this, new EchoPointEventArgs(m_prevPoints[0]));
m_prevPoints.Add(currPts[0]);
TouchDown?.Invoke(this, new EchoPointEventArgs(currPts[0]));
}
else
{
// the other one is new
currPts[1].Id = correctId;
m_prevPoints[0].Update(currPts[0]);
TouchMove?.Invoke(this, new EchoPointEventArgs(m_prevPoints[0]));
m_prevPoints.Add(currPts[1]);
TouchDown?.Invoke(this, new EchoPointEventArgs(currPts[1]));
}
}
}
else if (m_prevPoints.Count == 2)
{
// there were two previous points
if (currPts.Count == 0)
{
// the points disappeared
TouchUp?.Invoke(this, new EchoPointEventArgs(m_prevPoints[0]));
TouchUp?.Invoke(this, new EchoPointEventArgs(m_prevPoints[1]));
m_prevPoints.Clear();
}
else if (currPts.Count == 1)
{
// we removed only one point
// find out which one
double distance1 = Math.Abs(currPts[0].Distance - m_prevPoints[0].Distance);
double distance2 = Math.Abs(currPts[0].Distance - m_prevPoints[1].Distance);
if (distance1 > distance2)
{
// the first previous is gone
m_prevPoints[1].Update(currPts[0]);
TouchMove?.Invoke(this, new EchoPointEventArgs(m_prevPoints[1]));
TouchUp?.Invoke(this, new EchoPointEventArgs(m_prevPoints[0]));
m_prevPoints.RemoveAt(0);
}
else
{
// the second previous is gone
m_prevPoints[0].Update(currPts[0]);
TouchMove?.Invoke(this, new EchoPointEventArgs(m_prevPoints[0]));
TouchUp?.Invoke(this, new EchoPointEventArgs(m_prevPoints[1]));
m_prevPoints.RemoveAt(1);
}
}
else
{
// both are still here -> update them
bool prevFlipped = m_prevPoints[0].Distance > m_prevPoints[1].Distance;
bool currFlipped = currPts[0].Distance > currPts[1].Distance;
if ((!prevFlipped && !currFlipped) ||
(prevFlipped && currFlipped))
{
m_prevPoints[0].Update(currPts[0]);
m_prevPoints[1].Update(currPts[1]);
TouchMove?.Invoke(this, new EchoPointEventArgs(m_prevPoints[0]));
TouchMove?.Invoke(this, new EchoPointEventArgs(m_prevPoints[1]));
}
else if ((!prevFlipped && currFlipped) ||
(prevFlipped && !currFlipped))
{
m_prevPoints[0].Update(currPts[1]);
m_prevPoints[1].Update(currPts[0]);
TouchMove?.Invoke(this, new EchoPointEventArgs(m_prevPoints[0]));
TouchMove?.Invoke(this, new EchoPointEventArgs(m_prevPoints[1]));
}
}
}
}
