Add TS4231 data source that automatically calculates 3D positions

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))]

OpenEphys.Onix/OpenEphys.Onix/ConfigureTS4231.cs → 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<ContextTask> Process(IObservable<ContextTask> 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))
             {
             }
         }

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);

OpenEphys.Onix/OpenEphys.Onix/TS4231Data.cs → OpenEphys.Onix/OpenEphys.Onix/TS4231V1Data.cs

@@ -6,19 +6,20 @@
 
 namespace OpenEphys.Onix
 {
-    public class TS4231Data : Source<TS4231DataFrame>
+    public class TS4231V1Data : Source<TS4231V1DataFrame>
     {
-        [TypeConverter(typeof(TS4231.NameConverter))]
+        [TypeConverter(typeof(TS4231V1.NameConverter))]
         public string DeviceName { get; set; }
 
-        public override IObservable<TS4231DataFrame> Generate()
+        public override IObservable<TS4231V1DataFrame> Generate()
         {
             return DeviceManager.GetDevice(DeviceName).SelectMany(deviceInfo =>
             {
-                var device = deviceInfo.GetDeviceContext(typeof(TS4231));
+                var device = deviceInfo.GetDeviceContext(typeof(TS4231V1));
+                var hubClockPerioduSec = 1e6 / device.Hub.ClockHz;
                 return deviceInfo.Context.FrameReceived
                     .Where(frame => frame.DeviceAddress == device.Address)
-                    .Select(frame => new TS4231DataFrame(frame));
+                    .Select(frame => new TS4231V1DataFrame(frame, hubClockPerioduSec));
             });
         }
     }

OpenEphys.Onix/OpenEphys.Onix/TS4231DataFrame.cs → 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 hubClockPerioduSec)
             : base(frame.Clock)
         {
             var payload = (TS4231Payload*)frame.Data.ToPointer();
             HubClock = payload->HubClock;
             SensorIndex = payload->SensorIndex;
-            EnvelopeWidth = payload->EnvelopeWidth;
+            EnvelopeWidth = hubClockPerioduSec * 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,
     }
 }

OpenEphys.Onix/OpenEphys.Onix/TS4231V1GeometricPositionConverter.cs

@@ -0,0 +1,169 @@
+using OpenCV.Net;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Numerics;
+using System.Reactive.Linq;
+
+namespace OpenEphys.Onix
+{
+    class TS4231PulseQueue
+    {
+        public Queue<double> PulseTimes { get; } = new(new double[TS4231V1GeometricPositionConverter.ValidPulseSequenceTemplate.Length / 4]);
+
+        public Queue<double> PulseWidths { get; } = new(new double[TS4231V1GeometricPositionConverter.ValidPulseSequenceTemplate.Length / 4]);
+
+        public Queue<bool> PulseParse { get; } = new(new bool[TS4231V1GeometricPositionConverter.ValidPulseSequenceTemplate.Length]);
+
+        public Queue<ulong> PulseDataClock { get; } = new(new ulong[TS4231V1GeometricPositionConverter.ValidPulseSequenceTemplate.Length / 4]);
+
+        public Queue<ulong> 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<int, TS4231PulseQueue> PulseQueues = new();
+
+        public TS4231V1GeometricPositionConverter(uint hubClockFrequencyHz, Point3d baseSation1Origin, Point3d baseSation2Origin)
+        {
+            HubClockFrequencyPeriod = 1d / hubClockFrequencyHz;
+
+            p = new Mat(3, 1, Depth.F64, 1);
+            p[0] = new Scalar(baseSation1Origin.X);
+            p[1] = new Scalar(baseSation1Origin.Y);
+            p[2] = new Scalar(baseSation1Origin.Z);
+
+            q = new Mat(3, 1, Depth.F64, 1);
+            q[0] = new Scalar(baseSation2Origin.X);
+            q[1] = new Scalar(baseSation2Origin.Y);
+            q[2] = new Scalar(baseSation2Origin.Z);
+        }
+
+        public unsafe TS4231V1GeometricPositionDataFrame Convert(oni.Frame frame)
+        {
+            var payload = (TS4231Payload*)frame.Data.ToPointer();
+
+            if (!PulseQueues.ContainsKey(payload->SensorIndex))
+                PulseQueues.Add(payload->SensorIndex, new TS4231PulseQueue());
+
+            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))
+            {
+                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));
+
+        }
+    }
+}

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<TS4231V1GeometricPositionDataFrame>
+    {
+        [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<TS4231V1GeometricPositionDataFrame> Generate()
+        {
+            return DeviceManager.GetDevice(DeviceName).SelectMany(
+                deviceInfo => Observable.Create<TS4231V1GeometricPositionDataFrame>(observer =>
+                {
+                    var device = deviceInfo.GetDeviceContext(typeof(TS4231V1));
+                    var pulseConverter = new TS4231V1GeometricPositionConverter(device.Hub.ClockHz, P, Q);
+
+                    var frameObserver = Observer.Create<oni.Frame>(
+                        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);
+                }));
+        }
+    }
+}

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; }
+
+    }
+}