Skip to content

Commit 18eeb02

Browse files
committed
Start adding tracer and watchdog
1 parent 0aaaa30 commit 18eeb02

File tree

2 files changed

+258
-0
lines changed

2 files changed

+258
-0
lines changed

src/wpilibsharp/Tracer.cs

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
using System.Globalization;
2+
using System.Text;
3+
using UnitsNet;
4+
using UnitsNet.NumberExtensions.NumberToDuration;
5+
6+
namespace WPILib;
7+
8+
public sealed class Tracer
9+
{
10+
private static readonly Duration MinPrintPeriod = 1.0.Seconds();
11+
12+
private Duration m_lastEpochsPrintTime;
13+
private Duration m_startTime;
14+
15+
private readonly Dictionary<string, Duration> m_epochs = [];
16+
17+
public Tracer()
18+
{
19+
ResetTimer();
20+
}
21+
22+
public void ClearEpochs()
23+
{
24+
m_epochs.Clear();
25+
ResetTimer();
26+
}
27+
28+
public void ResetTimer()
29+
{
30+
m_startTime = Timer.FPGATimestamp;
31+
}
32+
33+
public void AddEpoch(string epochName)
34+
{
35+
var currentTime = Timer.FPGATimestamp;
36+
m_epochs.Add(epochName, currentTime - m_startTime);
37+
m_startTime = currentTime;
38+
}
39+
40+
public void PrintEpochs()
41+
{
42+
PrintEpochs(o => DriverStation.ReportWarning(o, false));
43+
}
44+
45+
public void PrintEpochs(Action<string> output)
46+
{
47+
var now = Timer.FPGATimestamp;
48+
if (now - m_lastEpochsPrintTime > MinPrintPeriod)
49+
{
50+
StringBuilder sb = new StringBuilder();
51+
m_lastEpochsPrintTime = now;
52+
foreach (var item in m_epochs)
53+
{
54+
sb.AppendLine(CultureInfo.InvariantCulture, $"\t{item.Key}: {item.Value}");
55+
}
56+
if (sb.Length > 0)
57+
{
58+
output(sb.ToString());
59+
}
60+
}
61+
}
62+
}

src/wpilibsharp/Watchdog.cs

Lines changed: 196 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,196 @@
1+
using System.Runtime.CompilerServices;
2+
using UnitsNet;
3+
using UnitsNet.NumberExtensions.NumberToDuration;
4+
using WPIHal.Handles;
5+
using WPIHal.Natives;
6+
using WPIUtil;
7+
8+
namespace WPILib;
9+
10+
public sealed class Watchdog : IDisposable
11+
{
12+
private static readonly Duration MinPrintPeriod = 1.Seconds();
13+
14+
private Duration m_startTime;
15+
private Duration m_timeout;
16+
private Duration m_expirationTime;
17+
private readonly Action m_callback;
18+
private Duration m_lastTimeoutPrint;
19+
20+
internal bool m_isExpired;
21+
22+
private readonly Tracer m_tracer;
23+
24+
private static readonly PriorityQueue<Watchdog, Duration> m_watchdogs = new();
25+
private static readonly object m_queueMutex = new();
26+
private static HalNotifierHandle m_notifier;
27+
28+
static Watchdog()
29+
{
30+
m_notifier = HalNotifier.InitializeNotifier();
31+
HalNotifier.SetNotifierName(m_notifier, "Watchdog");
32+
StartDaemonThread(SchedulerFunc);
33+
}
34+
35+
public Watchdog(Duration timeout, Action callback)
36+
{
37+
m_timeout = timeout;
38+
m_callback = WpiGuard.RequireNotNull(callback);
39+
m_tracer = new();
40+
}
41+
42+
public void Dispose()
43+
{
44+
Disable();
45+
}
46+
47+
public Duration Time => Timer.FPGATimestamp - m_startTime;
48+
49+
public Duration Timeout
50+
{
51+
get
52+
{
53+
lock (m_queueMutex)
54+
{
55+
return m_timeout;
56+
}
57+
}
58+
set
59+
{
60+
m_startTime = Timer.FPGATimestamp;
61+
m_tracer.ClearEpochs();
62+
63+
lock (m_queueMutex)
64+
{
65+
m_timeout = value;
66+
m_isExpired = false;
67+
68+
// TODO remove from queue
69+
m_expirationTime = m_startTime + m_timeout;
70+
m_watchdogs.Enqueue(this, m_expirationTime);
71+
UpdateAlarm();
72+
}
73+
}
74+
}
75+
76+
public bool IsExpired
77+
{
78+
get
79+
{
80+
lock (m_queueMutex)
81+
{
82+
return m_isExpired;
83+
}
84+
}
85+
}
86+
87+
public void AddEpoch(string epochName)
88+
{
89+
m_tracer.AddEpoch(epochName);
90+
}
91+
92+
public void PrintEpochs()
93+
{
94+
m_tracer.PrintEpochs();
95+
}
96+
97+
public void Reset()
98+
{
99+
Enable();
100+
}
101+
102+
public void Enable()
103+
{
104+
m_startTime = Timer.FPGATimestamp;
105+
m_tracer.ClearEpochs();
106+
107+
lock (m_queueMutex)
108+
{
109+
m_isExpired = false;
110+
111+
// TODO remove
112+
m_expirationTime = m_startTime + m_timeout;
113+
m_watchdogs.Enqueue(this, m_expirationTime);
114+
UpdateAlarm();
115+
}
116+
}
117+
118+
public void Disable()
119+
{
120+
lock (m_queueMutex)
121+
{
122+
GC.KeepAlive(this);
123+
UpdateAlarm();
124+
}
125+
}
126+
127+
public bool SuppressTimeoutMessage { get; set; }
128+
129+
private static void UpdateAlarm()
130+
{
131+
if (m_watchdogs.Count == 0)
132+
{
133+
HalNotifier.CancelNotifierAlarm(m_notifier);
134+
}
135+
else
136+
{
137+
HalNotifier.UpdateNotifierAlarm(m_notifier, (ulong)m_watchdogs.Peek().m_expirationTime.Microseconds);
138+
}
139+
}
140+
141+
private static Thread StartDaemonThread(ThreadStart target)
142+
{
143+
Thread inst = new(target)
144+
{
145+
IsBackground = true
146+
};
147+
inst.Start();
148+
return inst;
149+
}
150+
151+
private static void SchedulerFunc()
152+
{
153+
while (true)
154+
{
155+
ulong curTime = HalNotifier.WaitForNotifierAlarm(m_notifier);
156+
if (curTime == 0)
157+
{
158+
break;
159+
}
160+
161+
Watchdog watchdog;
162+
163+
lock (m_queueMutex)
164+
{
165+
if (m_watchdogs.Count == 0)
166+
{
167+
continue;
168+
}
169+
170+
// If the condition variable timed out, that means a Watchdog timeout
171+
// has occurred, so call its timeout function.
172+
watchdog = m_watchdogs.Dequeue();
173+
174+
Duration now = curTime.Microseconds();
175+
if (now - watchdog.m_lastTimeoutPrint > MinPrintPeriod)
176+
{
177+
watchdog.m_lastTimeoutPrint = now;
178+
if (!watchdog.SuppressTimeoutMessage)
179+
{
180+
DriverStation.ReportWarning($"Watchdog not fed within {watchdog.m_timeout}", false);
181+
}
182+
}
183+
184+
// Set expiration flag before calling the callback so any
185+
// manipulation of the flag in the callback (e.g., calling
186+
// Disable()) isn't clobbered.
187+
watchdog.m_isExpired = true;
188+
}
189+
watchdog.m_callback();
190+
lock (m_queueMutex)
191+
{
192+
UpdateAlarm();
193+
}
194+
}
195+
}
196+
}

0 commit comments

Comments
 (0)