diff --git a/OpenEphys.Onix/OpenEphys.Onix/ConfigureHeadstage64.cs b/OpenEphys.Onix/OpenEphys.Onix/ConfigureHeadstage64.cs index 6263a948..8f9789ad 100644 --- a/OpenEphys.Onix/OpenEphys.Onix/ConfigureHeadstage64.cs +++ b/OpenEphys.Onix/OpenEphys.Onix/ConfigureHeadstage64.cs @@ -29,7 +29,7 @@ public ConfigureHeadstage64() [Category(ConfigurationCategory)] [TypeConverter(typeof(HubDeviceConverter))] - public ConfigureTS4231 TS4231 { get; set; } = new() { Enable = false }; + public ConfigureTS4231V1 TS4231 { get; set; } = new() { Enable = false }; [Category(ConfigurationCategory)] [TypeConverter(typeof(HubDeviceConverter))] diff --git a/OpenEphys.Onix/OpenEphys.Onix/ConfigureTS4231.cs b/OpenEphys.Onix/OpenEphys.Onix/ConfigureTS4231V1.cs similarity index 79% rename from OpenEphys.Onix/OpenEphys.Onix/ConfigureTS4231.cs rename to OpenEphys.Onix/OpenEphys.Onix/ConfigureTS4231V1.cs index 0c6577e1..cea173b6 100644 --- a/OpenEphys.Onix/OpenEphys.Onix/ConfigureTS4231.cs +++ b/OpenEphys.Onix/OpenEphys.Onix/ConfigureTS4231V1.cs @@ -3,10 +3,10 @@ namespace OpenEphys.Onix { - public class ConfigureTS4231 : SingleDeviceFactory + public class ConfigureTS4231V1 : SingleDeviceFactory { - public ConfigureTS4231() - : base(typeof(TS4231)) + public ConfigureTS4231V1() + : base(typeof(TS4231V1)) { } @@ -21,13 +21,13 @@ public override IObservable Process(IObservable source return source.ConfigureDevice(context => { var device = context.GetDeviceContext(deviceAddress, DeviceType); - device.WriteRegister(TS4231.ENABLE, Enable ? 1u : 0); + device.WriteRegister(TS4231V1.ENABLE, Enable ? 1u : 0); return DeviceManager.RegisterDevice(deviceName, device, DeviceType); }); } } - static class TS4231 + static class TS4231V1 { public const int ID = 25; @@ -37,7 +37,7 @@ static class TS4231 internal class NameConverter : DeviceNameConverter { public NameConverter() - : base(typeof(TS4231)) + : base(typeof(TS4231V1)) { } } diff --git a/OpenEphys.Onix/OpenEphys.Onix/DeviceContext.cs b/OpenEphys.Onix/OpenEphys.Onix/DeviceContext.cs index 53f24b4a..6102d1b2 100644 --- a/OpenEphys.Onix/OpenEphys.Onix/DeviceContext.cs +++ b/OpenEphys.Onix/OpenEphys.Onix/DeviceContext.cs @@ -19,6 +19,8 @@ public DeviceContext(ContextTask context, oni.Device device) public oni.Device DeviceMetadata => _device; + public oni.Hub Hub => _context.GetHub(_device.Address); + public uint ReadRegister(uint registerAddress) { return _context.ReadRegister(_device.Address, registerAddress); diff --git a/OpenEphys.Onix/OpenEphys.Onix/TS4231Data.cs b/OpenEphys.Onix/OpenEphys.Onix/TS4231V1Data.cs similarity index 60% rename from OpenEphys.Onix/OpenEphys.Onix/TS4231Data.cs rename to OpenEphys.Onix/OpenEphys.Onix/TS4231V1Data.cs index 52faa883..2b6b7f12 100644 --- a/OpenEphys.Onix/OpenEphys.Onix/TS4231Data.cs +++ b/OpenEphys.Onix/OpenEphys.Onix/TS4231V1Data.cs @@ -6,19 +6,20 @@ namespace OpenEphys.Onix { - public class TS4231Data : Source + public class TS4231V1Data : Source { - [TypeConverter(typeof(TS4231.NameConverter))] + [TypeConverter(typeof(TS4231V1.NameConverter))] public string DeviceName { get; set; } - public override IObservable Generate() + public override IObservable Generate() { return DeviceManager.GetDevice(DeviceName).SelectMany(deviceInfo => { - var device = deviceInfo.GetDeviceContext(typeof(TS4231)); + var device = deviceInfo.GetDeviceContext(typeof(TS4231V1)); + var hubClockPeriod = 1e6 / device.Hub.ClockHz; return deviceInfo.Context.FrameReceived .Where(frame => frame.DeviceAddress == device.Address) - .Select(frame => new TS4231DataFrame(frame)); + .Select(frame => new TS4231V1DataFrame(frame, hubClockPeriod)); }); } } diff --git a/OpenEphys.Onix/OpenEphys.Onix/TS4231DataFrame.cs b/OpenEphys.Onix/OpenEphys.Onix/TS4231V1DataFrame.cs similarity index 54% rename from OpenEphys.Onix/OpenEphys.Onix/TS4231DataFrame.cs rename to OpenEphys.Onix/OpenEphys.Onix/TS4231V1DataFrame.cs index 4dc938d5..97d877ae 100644 --- a/OpenEphys.Onix/OpenEphys.Onix/TS4231DataFrame.cs +++ b/OpenEphys.Onix/OpenEphys.Onix/TS4231V1DataFrame.cs @@ -1,24 +1,26 @@ -using System.Runtime.InteropServices; +using System.Numerics; +using System.Runtime.InteropServices; +using oni; namespace OpenEphys.Onix { - public class TS4231DataFrame : DataFrame + public class TS4231V1DataFrame : DataFrame { - public unsafe TS4231DataFrame(oni.Frame frame) + public unsafe TS4231V1DataFrame(oni.Frame frame, double hubClockPeriod) : base(frame.Clock) { var payload = (TS4231Payload*)frame.Data.ToPointer(); HubClock = payload->HubClock; SensorIndex = payload->SensorIndex; - EnvelopeWidth = payload->EnvelopeWidth; + EnvelopeWidth = hubClockPeriod * payload->EnvelopeWidth; EnvelopeType = payload->EnvelopeType; } public int SensorIndex { get; } - public uint EnvelopeWidth { get; } + public double EnvelopeWidth { get; } - public TS4231Envelope EnvelopeType { get; } + public TS4231V1Envelope EnvelopeType { get; } } [StructLayout(LayoutKind.Sequential, Pack = 1)] @@ -27,17 +29,20 @@ struct TS4231Payload public ulong HubClock; public ushort SensorIndex; public uint EnvelopeWidth; - public TS4231Envelope EnvelopeType; + public TS4231V1Envelope EnvelopeType; } - public enum TS4231Envelope : short + public enum TS4231V1Envelope : short { - Sweep, + Bad = -1, J0, K0, J1, K1, J2, - K2 + K2, + J3, + K3, + Sweep, } } diff --git a/OpenEphys.Onix/OpenEphys.Onix/TS4231V1GeometricPositionConverter.cs b/OpenEphys.Onix/OpenEphys.Onix/TS4231V1GeometricPositionConverter.cs new file mode 100644 index 00000000..6d4fa841 --- /dev/null +++ b/OpenEphys.Onix/OpenEphys.Onix/TS4231V1GeometricPositionConverter.cs @@ -0,0 +1,171 @@ +using OpenCV.Net; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Numerics; +using System.Reactive.Linq; + +namespace OpenEphys.Onix +{ + class TS4231V1PulseQueue + { + public Queue PulseTimes { get; } = new(new double[TS4231V1GeometricPositionConverter.ValidPulseSequenceTemplate.Length / 4]); + + public Queue PulseWidths { get; } = new(new double[TS4231V1GeometricPositionConverter.ValidPulseSequenceTemplate.Length / 4]); + + public Queue PulseParse { get; } = new(new bool[TS4231V1GeometricPositionConverter.ValidPulseSequenceTemplate.Length]); + + public Queue PulseDataClock { get; } = new(new ulong[TS4231V1GeometricPositionConverter.ValidPulseSequenceTemplate.Length / 4]); + + public Queue PulseFrameClock { get; } = new(new ulong[TS4231V1GeometricPositionConverter.ValidPulseSequenceTemplate.Length / 4]); + } + + class TS4231V1GeometricPositionConverter + { + const double SweepFrequencyHz = 60; + readonly double HubClockFrequencyPeriod; + readonly Mat p; + readonly Mat q; + + // Template pattern + internal static readonly bool[] ValidPulseSequenceTemplate = { + // bad skip axis sweep + false, false, false, false, + false, true, false, false, + false, false, false, true, // axis 0, station 0 + false, false, true, false, + false, true, true, false, + false, false, false, true, // axis 1, station 0 + false, true, false, false, + false, false, false, false, + false, false, false, true, // axis 0, station 1 + false, true, true, false, + false, false, true, false, + false, false, false, true // axis 1, station 1 + }; + + Dictionary PulseQueues = new(); + + public TS4231V1GeometricPositionConverter(uint hubClockFrequencyHz, Point3d baseStation1Origin, Point3d baseStation2Origin) + { + HubClockFrequencyPeriod = 1d / hubClockFrequencyHz; + + p = new Mat(3, 1, Depth.F64, 1); + p[0] = new Scalar(baseStation1Origin.X); + p[1] = new Scalar(baseStation1Origin.Y); + p[2] = new Scalar(baseStation1Origin.Z); + + q = new Mat(3, 1, Depth.F64, 1); + q[0] = new Scalar(baseStation2Origin.X); + q[1] = new Scalar(baseStation2Origin.Y); + q[2] = new Scalar(baseStation2Origin.Z); + } + + public unsafe TS4231V1GeometricPositionDataFrame Convert(oni.Frame frame) + { + var payload = (TS4231Payload*)frame.Data.ToPointer(); + + if (!PulseQueues.ContainsKey(payload->SensorIndex)) + PulseQueues.Add(payload->SensorIndex, new TS4231V1PulseQueue()); + + var queues = PulseQueues[payload->SensorIndex]; + + // Push pulse time into buffer and pop oldest + queues.PulseTimes.Dequeue(); + queues.PulseTimes.Enqueue(HubClockFrequencyPeriod * payload->HubClock); + + queues.PulseDataClock.Dequeue(); + queues.PulseDataClock.Enqueue(payload->HubClock); + + queues.PulseFrameClock.Dequeue(); + queues.PulseFrameClock.Enqueue(frame.Clock); + + // Push pulse width into buffer and pop oldest + queues.PulseWidths.Dequeue(); + queues.PulseWidths.Enqueue(HubClockFrequencyPeriod * payload->EnvelopeWidth); + + // push pulse code categorization into buffer and pop oldest 4x + queues.PulseParse.Dequeue(); + queues.PulseParse.Dequeue(); + queues.PulseParse.Dequeue(); + queues.PulseParse.Dequeue(); + queues.PulseParse.Enqueue(payload->EnvelopeType == TS4231V1Envelope.Bad); + queues.PulseParse.Enqueue(payload->EnvelopeType >= TS4231V1Envelope.J2 & payload->EnvelopeType != TS4231V1Envelope.Sweep); // skip + queues.PulseParse.Enqueue((int)payload->EnvelopeType % 2 == 1 & payload->EnvelopeType != TS4231V1Envelope.Sweep); // axis + queues.PulseParse.Enqueue(payload->EnvelopeType == TS4231V1Envelope.Sweep); // sweep + + // test template match and make sure time between pulses does not integrate to more than two periods + if (!queues.PulseParse.SequenceEqual(ValidPulseSequenceTemplate) || + queues.PulseTimes.Last() - queues.PulseTimes.First() > 2 / SweepFrequencyHz) + { + return null; + } + + // position measurement time is defined to be the mean of the data used + var time = queues.PulseTimes.ToArray(); + var width = queues.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 * SweepFrequencyHz * t11 - Math.PI / 2; + var gamma0 = 2 * Math.PI * SweepFrequencyHz * 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 * SweepFrequencyHz * t12 - Math.PI / 2; + var gamma1 = 2 * Math.PI * SweepFrequencyHz * 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; + + // If singular, return null + if (double.IsNaN(x1) || + double.IsNaN(x2) || + double.IsInfinity(x1) || + double.IsInfinity(x2)) + { + return null; + } + + // calculate position + var p1 = p + x1 * u; + var q1 = q + x2 * v; + var position = 0.5 * (p1 + q1); + + return new TS4231V1GeometricPositionDataFrame(queues.PulseDataClock.ElementAt(ValidPulseSequenceTemplate.Length / 8), + queues.PulseFrameClock.ElementAt(ValidPulseSequenceTemplate.Length / 8), + payload->SensorIndex, + new Vector3((float)position[0].Val0, (float)position[1].Val0, (float)position[2].Val0)); + } + } +} diff --git a/OpenEphys.Onix/OpenEphys.Onix/TS4231V1GeometricPositionData.cs b/OpenEphys.Onix/OpenEphys.Onix/TS4231V1GeometricPositionData.cs new file mode 100644 index 00000000..b40c0b8e --- /dev/null +++ b/OpenEphys.Onix/OpenEphys.Onix/TS4231V1GeometricPositionData.cs @@ -0,0 +1,46 @@ +using System; +using System.ComponentModel; +using System.Linq; +using System.Reactive; +using System.Reactive.Linq; +using Bonsai; +using OpenCV.Net; + +namespace OpenEphys.Onix +{ + public class TS4231V1GeometricPositionData : Source + { + [TypeConverter(typeof(TS4231V1.NameConverter))] + public string DeviceName { get; set; } + + public Point3d P { get; set; } = new(0, 0, 0); + + public Point3d Q { get; set; } = new(1, 0, 0); + + public unsafe override IObservable Generate() + { + return DeviceManager.GetDevice(DeviceName).SelectMany( + deviceInfo => Observable.Create(observer => + { + var device = deviceInfo.GetDeviceContext(typeof(TS4231V1)); + var pulseConverter = new TS4231V1GeometricPositionConverter(device.Hub.ClockHz, P, Q); + + var frameObserver = Observer.Create( + frame => + { + var position = pulseConverter.Convert(frame); + if (position != null) + { + observer.OnNext(position); + } + }, + observer.OnError, + observer.OnCompleted); + + return deviceInfo.Context.FrameReceived + .Where(frame => frame.DeviceAddress == device.Address) + .SubscribeSafe(frameObserver); + })); + } + } +} diff --git a/OpenEphys.Onix/OpenEphys.Onix/TS4231V1GeometricPositionDataFrame.cs b/OpenEphys.Onix/OpenEphys.Onix/TS4231V1GeometricPositionDataFrame.cs new file mode 100644 index 00000000..5d318dab --- /dev/null +++ b/OpenEphys.Onix/OpenEphys.Onix/TS4231V1GeometricPositionDataFrame.cs @@ -0,0 +1,24 @@ +using System.Numerics; + +namespace OpenEphys.Onix +{ + public class TS4231V1GeometricPositionDataFrame + { + public TS4231V1GeometricPositionDataFrame(ulong clock, ulong hubClock, int sensorIndex, Vector3 position) + { + Clock = clock; + HubClock = hubClock; + SensorIndex = sensorIndex; + Position = position; + } + + public ulong Clock { get; } + + public ulong HubClock { get; } + + public int SensorIndex { get; } + + public Vector3 Position { get; } + + } +}