Finish up Addressable LED Sim

robotdotnet · Mar 19, 2024 · 34b202a · 34b202a
1 parent 0346a73
commit 34b202a
Show file tree

Hide file tree

Showing 3 changed files with 161 additions and 2 deletions.
diff --git a/src/hal/Natives/Simulation/HalAddressableLEDData.cs b/src/hal/Natives/Simulation/HalAddressableLEDData.cs
@@ -92,11 +92,22 @@ public static unsafe partial class HalAddressableLEDData
 
     [LibraryImport("wpiHal", EntryPoint = "HALSIM_GetAddressableLEDData")]
     [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
-    public static partial int GetData(int index, HalAddressableLED.LedData* data);
+    public static partial int GetDataInternal(int index, Span<HalAddressableLED.LedData> data);
+
+    public static ReadOnlySpan<HalAddressableLED.LedData> GetData(int index, Span<HalAddressableLED.LedData> data)
+    {
+        int length = GetDataInternal(index, data);
+        return data[..length];
+    }
 
     [LibraryImport("wpiHal", EntryPoint = "HALSIM_SetAddressableLEDData")]
     [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
-    public static partial void SetData(int index, HalAddressableLED.LedData* data, int length);
+    public static partial void SetData(int index, ReadOnlySpan<HalAddressableLED.LedData> data, int length);
+
+    public static void SetData(int index, ReadOnlySpan<HalAddressableLED.LedData> data)
+    {
+        SetData(index, data, data.Length);
+    }
 
     [LibraryImport("wpiHal", EntryPoint = "HALSIM_RegisterAddressableLEDAllCallbacks")]
     [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]

diff --git a/src/wpilibsharp/AddressableLED.cs b/src/wpilibsharp/AddressableLED.cs
@@ -0,0 +1,63 @@
+using WPIHal.Handles;
+using WPIHal.Natives;
+
+namespace WPILib;
+
+public class AddressableLED : IDisposable
+{
+    private readonly HalDigitalHandle m_pwmHandle;
+    private readonly HalAddressableLEDHandle m_handle;
+
+    public AddressableLED(int port)
+    {
+        m_pwmHandle = HalPWM.InitializePWMPort(HalBase.GetPort(port), Environment.StackTrace);
+        m_handle = HalAddressableLED.InitializeAddressableLED(m_pwmHandle);
+        // TODO report
+    }
+
+    public void Dispose()
+    {
+        GC.SuppressFinalize(this);
+        if (m_handle.Handle != 0)
+        {
+            HalAddressableLED.FreeAddressableLED(m_handle);
+        }
+        if (m_pwmHandle.Handle != 0)
+        {
+            HalPWM.FreePWMPort(m_pwmHandle);
+        }
+    }
+
+    public int Length
+    {
+        set
+        {
+            HalAddressableLED.SetAddressableLEDLength(m_handle, value);
+        }
+    }
+
+    public void SetData(ReadOnlySpan<HalAddressableLED.LedData> buffer)
+    {
+        HalAddressableLED.WriteAddressableLEDData(m_handle, buffer);
+    }
+
+    public void SetBitTiming(TimeSpan highTime0, TimeSpan lowTime0, TimeSpan highTime1, TimeSpan lowTime1)
+    {
+        HalAddressableLED.SetAddressableLEDBitTiming(m_handle, (int)highTime0.TotalNanoseconds, (int)lowTime0.TotalNanoseconds, (int)highTime1.TotalNanoseconds, (int)lowTime1.TotalNanoseconds);
+    }
+
+    public void SetSyncTime(TimeSpan syncTime)
+    {
+        HalAddressableLED.SetAddressableLEDSyncTime(m_handle, (int)syncTime.TotalMicroseconds);
+    }
+
+    public void Start()
+    {
+        HalAddressableLED.StartAddressableLEDOutput(m_handle);
+    }
+
+    public void Stop()
+    {
+        HalAddressableLED.StopAddressableLEDOutput(m_handle);
+    }
+}
diff --git a/src/wpilibsharp/Simulation/AddressableLEDSim.cs b/src/wpilibsharp/Simulation/AddressableLEDSim.cs
@@ -1,4 +1,7 @@
+using CommunityToolkit.Diagnostics;
+using CommunityToolkit.HighPerformance;
 using WPIHal;
+using WPIHal.Natives;
 using WPIHal.Natives.Simulation;
 
 namespace WPILib.Simulation;
@@ -12,28 +15,110 @@ public AddressableLEDSim()
         m_index = 0;
     }
 
+    public AddressableLEDSim(AddressableLED addressableLED)
+    {
+        m_index = 0;
+    }
+
+    private AddressableLEDSim(int index)
+    {
+        m_index = index;
+    }
+
+    public static AddressableLEDSim CreateForChannel(int pwmChannel)
+    {
+        int index = HalAddressableLEDData.FindForChannel(pwmChannel);
+        if (index < 0)
+        {
+            ThrowHelper.ThrowArgumentOutOfRangeException(nameof(pwmChannel), $"No addressable LED found for PWM channel {pwmChannel}");
+        }
+        return new AddressableLEDSim(index);
+    }
+
+    public static AddressableLEDSim CreateForIndex(int index)
+    {
+        return new AddressableLEDSim(index);
+    }
+
     public unsafe CallbackStore RegisterInitializedCallback(NotifyCallback callback, bool initialNotify)
     {
         return new CallbackStore(callback, m_index, initialNotify, &HalAddressableLEDData.RegisterInitializedCallback, &HalAddressableLEDData.CancelInitializedCallback);
     }
 
+    public bool GetInitialized()
+    {
+        return HalAddressableLEDData.GetInitialized(m_index);
+    }
+
+    public void SetInitialized(bool initialized)
+    {
+        HalAddressableLEDData.SetInitialized(m_index, initialized);
+    }
+
     public unsafe CallbackStore RegisterOutputPortCallback(NotifyCallback callback, bool initialNotify)
     {
         return new CallbackStore(callback, m_index, initialNotify, &HalAddressableLEDData.RegisterOutputPortCallback, &HalAddressableLEDData.CancelOutputPortCallback);
     }
 
+    public int GetOutputPort()
+    {
+        return HalAddressableLEDData.GetOutputPort(m_index);
+    }
+
+    public void SetOutputPort(int outputPort)
+    {
+        HalAddressableLEDData.SetOutputPort(m_index, outputPort);
+    }
+
     public unsafe CallbackStore RegisterLengthCallback(NotifyCallback callback, bool initialNotify)
     {
         return new CallbackStore(callback, m_index, initialNotify, &HalAddressableLEDData.RegisterLengthCallback, &HalAddressableLEDData.CancelLengthCallback);
     }
 
+    public int GetLength()
+    {
+        return HalAddressableLEDData.GetLength(m_index);
+    }
+
+    public void SetLength(int length)
+    {
+        HalAddressableLEDData.SetLength(m_index, length);
+    }
+
     public unsafe CallbackStore RegisterRunningCallback(NotifyCallback callback, bool initialNotify)
     {
         return new CallbackStore(callback, m_index, initialNotify, &HalAddressableLEDData.RegisterRunningCallback, &HalAddressableLEDData.CancelRunningCallback);
     }
 
+    public bool GetRunning()
+    {
+        return HalAddressableLEDData.GetRunning(m_index);
+    }
+
+    public void SetRunning(bool running)
+    {
+        HalAddressableLEDData.SetRunning(m_index, running);
+    }
+
     public unsafe CallbackStore RegisterDataCallback(ConstBufferCallback callback)
     {
         return new CallbackStore(callback, m_index, &HalAddressableLEDData.RegisterDataCallback, &HalAddressableLEDData.CancelDataCallback);
     }
+
+    public Memory<HalAddressableLED.LedData> GetData()
+    {
+        HalAddressableLED.LedData[] data = new HalAddressableLED.LedData[5460];
+        var ret = HalAddressableLEDData.GetData(m_index, data.AsSpan());
+        return data.AsMemory()[..ret.Length];
+    }
+
+    public void SetData(ReadOnlySpan<HalAddressableLED.LedData> data)
+    {
+        HalAddressableLEDData.SetData(m_index, data);
+    }
+
+    public void ResetData()
+    {
+        HalAddressableLEDData.ResetData(m_index);
+    }
 }