Sim work

robotdotnet · Mar 19, 2024 · 0346a73 · 0346a73
1 parent 5755d97
commit 0346a73
Show file tree

Hide file tree

Showing 5 changed files with 45 additions and 26 deletions.
diff --git a/codehelp/CodeHelpers/LogGenerator/CodeFixer/LogGeneratorFixer.cs b/codehelp/CodeHelpers/LogGenerator/CodeFixer/LogGeneratorFixer.cs
@@ -23,16 +23,6 @@ public sealed override FixAllProvider GetFixAllProvider()
         return WellKnownFixAllProviders.BatchFixer;
     }
 
-    private SyntaxTrivia _detectLineBreakTrivia(SyntaxNode root)
-    {
-        var result = root.DescendantTokens()
-                         .SelectMany(token => token.TrailingTrivia)
-                          .FirstOrDefault(trivia => trivia.IsKind(SyntaxKind.EndOfLineTrivia));
-        if (Equals(result, default(SyntaxTrivia)))
-            result = SyntaxFactory.CarriageReturnLineFeed;
-        return result;
-    }
-
     public sealed override async Task RegisterCodeFixesAsync(CodeFixContext context)
     {
         var root = await context.Document.GetSyntaxRootAsync(context.CancellationToken).ConfigureAwait(false);

diff --git a/dev/desktopDev/Program.cs b/dev/desktopDev/Program.cs
@@ -22,13 +22,15 @@ static void Main(string[] args)
     {
         var arr = typeof(HalAccelerometerData).Assembly.GetTypes().Where(x => x.Name.StartsWith("Hal") && x.Name.EndsWith("Data")).ToArray();
 
-        foreach (var item in arr) {
+        foreach (var item in arr)
+        {
             var nameWithoutHal = item.Name[3..];
             var nameWithoutData = nameWithoutHal[..^4];
             Console.WriteLine(nameWithoutData);
             var file2 = $@"C:\Users\thadh\Documents\GitHub\robotdotnet\WPILib\src\hal\Natives\Simulation\{item.Name}.cs";
             var txt = File.ReadAllText(file2);
-            foreach (var func in item.GetMethods()) {
+            foreach (var func in item.GetMethods())
+            {
                 var funcName = func.Name.Replace(nameWithoutData, "");
 
                 txt = txt.Replace($"{func.Name}(", $"{funcName}(");

diff --git a/src/wpilibsharp/Simulation/AddressableLEDSim.cs b/src/wpilibsharp/Simulation/AddressableLEDSim.cs
@@ -14,26 +14,26 @@ public AddressableLEDSim()
 
     public unsafe CallbackStore RegisterInitializedCallback(NotifyCallback callback, bool initialNotify)
     {
-    return new CallbackStore(callback, m_index, initialNotify, &HalAddressableLEDData.RegisterInitializedCallback, &HalAddressableLEDData.CancelInitializedCallback);
+        return new CallbackStore(callback, m_index, initialNotify, &HalAddressableLEDData.RegisterInitializedCallback, &HalAddressableLEDData.CancelInitializedCallback);
     }
 
     public unsafe CallbackStore RegisterOutputPortCallback(NotifyCallback callback, bool initialNotify)
     {
-    return new CallbackStore(callback, m_index, initialNotify, &HalAddressableLEDData.RegisterOutputPortCallback, &HalAddressableLEDData.CancelOutputPortCallback);
+        return new CallbackStore(callback, m_index, initialNotify, &HalAddressableLEDData.RegisterOutputPortCallback, &HalAddressableLEDData.CancelOutputPortCallback);
     }
 
     public unsafe CallbackStore RegisterLengthCallback(NotifyCallback callback, bool initialNotify)
     {
-    return new CallbackStore(callback, m_index, initialNotify, &HalAddressableLEDData.RegisterLengthCallback, &HalAddressableLEDData.CancelLengthCallback);
+        return new CallbackStore(callback, m_index, initialNotify, &HalAddressableLEDData.RegisterLengthCallback, &HalAddressableLEDData.CancelLengthCallback);
     }
 
     public unsafe CallbackStore RegisterRunningCallback(NotifyCallback callback, bool initialNotify)
     {
-    return new CallbackStore(callback, m_index, initialNotify, &HalAddressableLEDData.RegisterRunningCallback, &HalAddressableLEDData.CancelRunningCallback);
+        return new CallbackStore(callback, m_index, initialNotify, &HalAddressableLEDData.RegisterRunningCallback, &HalAddressableLEDData.CancelRunningCallback);
     }
 
     public unsafe CallbackStore RegisterDataCallback(ConstBufferCallback callback)
     {
-    return new CallbackStore(callback, m_index, initialNotify, &HalAddressableLEDData.RegisterDataCallback, &HalAddressableLEDData.CancelDataCallback);
+        return new CallbackStore(callback, m_index, &HalAddressableLEDData.RegisterDataCallback, &HalAddressableLEDData.CancelDataCallback);
     }
 }
diff --git a/src/wpilibsharp/Simulation/CallbackStore.cs b/src/wpilibsharp/Simulation/CallbackStore.cs
@@ -1,6 +1,7 @@
 using System.Runtime.CompilerServices;
 using System.Runtime.InteropServices;
 using WPIHal;
+using unsafe HalConstBufferCreate = delegate* managed<int, delegate* unmanaged[Cdecl]<byte*, void*, byte*, uint, void>, void*, int>;
 using unsafe HalGlobalCreate = delegate* managed<delegate* unmanaged[Cdecl]<byte*, void*, WPIHal.HalValue*, void>, void*, bool, int>;
 using unsafe HalGlobalFree = delegate* managed<int, void>;
 using unsafe HalIndexedCreate = delegate* managed<int, delegate* unmanaged[Cdecl]<byte*, void*, WPIHal.HalValue*, void>, void*, bool, int>;
@@ -9,25 +10,40 @@
 
 namespace WPILib.Simulation;
 
-public class CallbackStore : IDisposable
+public delegate void NotifyCallback(string name, HalValue value);
+
+public delegate void ConstBufferCallback(string name, ReadOnlySpan<byte> buffer);
+
+public sealed class CallbackStore : IDisposable
 {
     private GCHandle delegateHandle;
     private readonly int nativeHandle;
     private readonly int? index;
-    private unsafe readonly HalIndexedFree indexedFree;
-    private unsafe readonly HalGlobalFree globalFree;
+    private readonly unsafe HalIndexedFree indexedFree;
+    private readonly unsafe HalGlobalFree globalFree;
 
     [UnmanagedCallersOnly(CallConvs = [typeof(CallConvCdecl)])]
     public static unsafe void HalNotifyCallback(byte* name, void* param, HalValue* value)
     {
         GCHandle handle = GCHandle.FromIntPtr((nint)param);
-        if (handle.Target is Action<string, HalValue> stringCallback)
+        if (handle.Target is NotifyCallback stringCallback)
         {
             string n = Marshal.PtrToStringUTF8((nint)name) ?? "";
             stringCallback(n, *value);
         }
     }
 
+    [UnmanagedCallersOnly(CallConvs = [typeof(CallConvCdecl)])]
+    public static unsafe void HalConstBufferCallback(byte* name, void* param, byte* buffer, uint len)
+    {
+        GCHandle handle = GCHandle.FromIntPtr((nint)param);
+        if (handle.Target is ConstBufferCallback stringCallback)
+        {
+            string n = Marshal.PtrToStringUTF8((nint)name) ?? "";
+            stringCallback(n, new ReadOnlySpan<byte>(buffer, (int)len));
+        }
+    }
+
     public unsafe void Dispose()
     {
         GC.SuppressFinalize(this);
@@ -42,19 +58,27 @@ public unsafe void Dispose()
         delegateHandle.Free();
     }
 
-    public unsafe CallbackStore(Action<string, HalValue> callback, bool immediateNotify, HalGlobalCreate create, HalGlobalFree free)
+    public unsafe CallbackStore(NotifyCallback callback, bool immediateNotify, HalGlobalCreate create, HalGlobalFree free)
     {
         delegateHandle = GCHandle.Alloc(callback);
         nativeHandle = create(&HalNotifyCallback, (void*)(nint)delegateHandle, immediateNotify);
         index = null;
         globalFree = free;
     }
 
-    public unsafe CallbackStore(Action<string, HalValue> callback, int index, bool immediateNotify, HalIndexedCreate create, HalIndexedFree free)
+    public unsafe CallbackStore(NotifyCallback callback, int index, bool immediateNotify, HalIndexedCreate create, HalIndexedFree free)
     {
         delegateHandle = GCHandle.Alloc(callback);
         nativeHandle = create(index, &HalNotifyCallback, (void*)(nint)delegateHandle, immediateNotify);
         this.index = index;
         indexedFree = free;
     }
+
+    public unsafe CallbackStore(ConstBufferCallback callback, int index, HalConstBufferCreate create, HalIndexedFree free)
+    {
+        delegateHandle = GCHandle.Alloc(callback);
+        nativeHandle = create(index, &HalConstBufferCallback, (void*)(nint)delegateHandle);
+        this.index = index;
+        indexedFree = free;
+    }
 }
diff --git a/src/wpimath/Interpolation/InterpolatingMap.cs b/src/wpimath/Interpolation/InterpolatingMap.cs
@@ -26,19 +26,22 @@ public T this[T key]
             }
 
             int idx = m_map.BinarySearch((key, T.Zero));
-            if (idx >= 0) {
+            if (idx >= 0)
+            {
                 return m_map[idx].value;
             }
 
             int larger = ~idx;
 
             // Request is smaller than all elements, return smallest
-            if (larger == 0) {
+            if (larger == 0)
+            {
                 return m_map[larger].value;
             }
 
             // Request is larger than all elements, return largest
-            if (larger == m_map.Count) {
+            if (larger == m_map.Count)
+            {
                 return m_map[^1].value;
             }