private bool Decode(TS4231V1DataFrame source)
{
// Push pulse time into buffer and pop oldest
pulseTimes.Dequeue();
pulseTimes.Enqueue(source.HubSyncCounter / DataClockHz ?? double.NaN);
pulseDataClock.Dequeue();
pulseDataClock.Enqueue(source.HubSyncCounter);
pulseFrameClock.Dequeue();
pulseFrameClock.Enqueue(source.Clock);
// Push pulse width into buffer and pop oldest
pulseWidths.Dequeue();
pulseWidths.Enqueue(source.PulseWidth / DataClockHz ?? double.NaN);
// Push pulse parse info into buffer and pop oldest 4x
pulseParse.Dequeue();
pulseParse.Dequeue();
pulseParse.Dequeue();
pulseParse.Dequeue();
var type = source.PulseType;
pulseParse.Enqueue(type == -1); // bad
pulseParse.Enqueue(type >= 4 & type != 8); // skip
pulseParse.Enqueue(type % 2 == 1 & type != 8); // axis
pulseParse.Enqueue(type == 8); // sweep
// Test template match and make sure time between pulses does
// not integrate to more than two periods
if (!pulseParse.SequenceEqual(Template) || pulseTimes.Last() - pulseTimes.First() > 2 / SweepFrequency)
{
return(false);
}
// Time is the mean of the data used
positionDataClock = pulseDataClock.ElementAt(Template.Length / 8);
positionFrameClock = pulseFrameClock.ElementAt(Template.Length / 8);
var time = pulseTimes.ToArray();
var width = pulseWidths.ToArray();
var t11 = time[2] + width[2] / 2 - time[0];
var t21 = time[5] + width[5] / 2 - time[3];
var theta0 = 2 * Math.PI * SweepFrequency * t11 - Math.PI / 2;
var gamma0 = 2 * Math.PI * SweepFrequency * t21 - Math.PI / 2;
var u = new Mat(3, 1, Depth.F64, 1);
u[0] = new Scalar(Math.Tan(theta0));
u[1] = new Scalar(Math.Tan(gamma0));
u[2] = new Scalar(1);
CV.Normalize(u, u);
var t12 = time[8] + width[8] / 2 - time[7];
var t22 = time[11] + width[11] / 2 - time[10];
var theta1 = 2 * Math.PI * SweepFrequency * t12 - Math.PI / 2;
var gamma1 = 2 * Math.PI * SweepFrequency * t22 - Math.PI / 2;
var v = new Mat(3, 1, Depth.F64, 1);
v[0] = new Scalar(Math.Tan(theta1));
v[1] = new Scalar(Math.Tan(gamma1));
v[2] = new Scalar(1);
CV.Normalize(v, v);
// Base station origin vector
var d = q - p;
// Linear transform
// A = [a11 a12]
// [a21 a22]
var a11 = 1.0;
var a12 = -CV.DotProduct(u, v);
var a21 = CV.DotProduct(u, v);
var a22 = -1.0;
// Result
// B = [b1]
// [b2]
var b1 = CV.DotProduct(u, d);
var b2 = CV.DotProduct(v, d);
// Solve Ax = B
var x2 = (b2 - (b1 * a21) / a11) / (a22 - (a12 * a21) / a11);
var x1 = (b1 - a12 * x2) / a11;
// TODO: If non-singular solution else send NaNs
//if (x)
//{
var p1 = p + x1 * u;
var q1 = q + x2 * v;
//}
// Or single matrix with columns as results
position = 0.5 * (p1 + q1);
return(true);
}