utils.loop_timing

  1"""
  2Continuous loop-timing instrumentation.
  3
  4Measures how much of the timed robot periodic budget the robot code consumes,
  5broken down by channel (e.g. userCode vs scheduler).  Stats are published
  6to SmartDashboard every cycle so they are always visible — not only on
  7overrun like the built-in CommandScheduler Watchdog.
  8
  9Timing uses FPGA timestamps (microsecond resolution on real hardware).
 10In simulation the values reflect simulated time, not wall-clock.
 11"""
 12
 13import math
 14import wpilib
 15
 16
 17class LoopTimingStats:
 18    """Running statistics accumulator (Welford's online algorithm).
 19
 20    All public values are in **milliseconds**.  Input to :meth:`record`
 21    is in seconds (matching ``Timer.getFPGATimestamp()`` units).
 22    """
 23
 24    def __init__(self):
 25        self.reset()
 26
 27    def reset(self):
 28        self._count: int = 0
 29        self._min: float = float("inf")
 30        self._max: float = float("-inf")
 31        self._last: float = 0.0
 32        # Welford accumulators
 33        self._mean: float = 0.0
 34        self._m2: float = 0.0
 35
 36    def record(self, duration_sec: float):
 37        """Record one sample.  *duration_sec* is converted to ms internally."""
 38        ms = duration_sec * 1000.0
 39        self._last = ms
 40        self._count += 1
 41
 42        if ms < self._min:
 43            self._min = ms
 44        if ms > self._max:
 45            self._max = ms
 46
 47        # Welford's online update
 48        delta = ms - self._mean
 49        self._mean += delta / self._count
 50        delta2 = ms - self._mean
 51        self._m2 += delta * delta2
 52
 53    @property
 54    def count(self) -> int:
 55        return self._count
 56
 57    @property
 58    def min_ms(self) -> float:
 59        return self._min if self._count > 0 else 0.0
 60
 61    @property
 62    def max_ms(self) -> float:
 63        return self._max if self._count > 0 else 0.0
 64
 65    @property
 66    def avg_ms(self) -> float:
 67        return self._mean if self._count > 0 else 0.0
 68
 69    @property
 70    def stddev_ms(self) -> float:
 71        if self._count < 2:
 72            return 0.0
 73        return math.sqrt(self._m2 / (self._count - 1))
 74
 75    @property
 76    def last_ms(self) -> float:
 77        return self._last
 78
 79
 80class LoopTimer:
 81    """Coordinator that manages named timing channels and publishes stats.
 82
 83    Typical usage::
 84
 85        timer = LoopTimer(budget_sec=0.020)
 86        timer.add_channel("userCode")
 87        timer.add_channel("scheduler")
 88
 89        # each cycle:
 90        timer.start("userCode")
 91        ...
 92        timer.stop("userCode")
 93        timer.publish()
 94    """
 95
 96    def __init__(self, budget_sec: float):
 97        self._budget_ms = budget_sec * 1000.0
 98        self._channels: dict[str, LoopTimingStats] = {}
 99        self._starts: dict[str, float] = {}
100        self._alert_info = wpilib.Alert(
101            "Frame Timing", wpilib.Alert.AlertType.kInfo
102        )
103        self._alert_warning = wpilib.Alert(
104            "Frame Timing", wpilib.Alert.AlertType.kWarning
105        )
106        self._alert_error = wpilib.Alert(
107            "Frame Timing", wpilib.Alert.AlertType.kError
108        )
109
110    def add_channel(self, name: str):
111        self._channels[name] = LoopTimingStats()
112
113    def start(self, name: str):
114        self._starts[name] = wpilib.Timer.getFPGATimestamp()
115
116    def stop(self, name: str):
117        end = wpilib.Timer.getFPGATimestamp()
118        begin = self._starts.pop(name, end)
119        self._channels[name].record(end - begin)
120
121    def reset_all(self):
122        for stats in self._channels.values():
123            stats.reset()
124        self._starts.clear()
125
126    def publish(self):
127        total_ms = 0.0
128        for name, stats in self._channels.items():
129            prefix = f"FrameTiming/{name}"
130            wpilib.SmartDashboard.putNumber(f"{prefix}/lastMs", stats.last_ms)
131            wpilib.SmartDashboard.putNumber(f"{prefix}/minMs", stats.min_ms)
132            wpilib.SmartDashboard.putNumber(f"{prefix}/maxMs", stats.max_ms)
133            wpilib.SmartDashboard.putNumber(f"{prefix}/avgMs", stats.avg_ms)
134            wpilib.SmartDashboard.putNumber(f"{prefix}/stddevMs", stats.stddev_ms)
135            wpilib.SmartDashboard.putNumber(f"{prefix}/count", stats.count)
136            if self._budget_ms > 0:
137                wpilib.SmartDashboard.putNumber(
138                    f"{prefix}/budgetPct",
139                    (stats.last_ms / self._budget_ms) * 100.0,
140                )
141            total_ms += stats.last_ms
142
143        wpilib.SmartDashboard.putNumber("FrameTiming/totalMs", total_ms)
144        total_pct = 0.0
145        if self._budget_ms > 0:
146            total_pct = (total_ms / self._budget_ms) * 100.0
147            wpilib.SmartDashboard.putNumber(
148                "FrameTiming/totalBudgetPct", total_pct,
149            )
150
151        # Alert based on budget usage
152        overrun = total_pct >= 100.0
153        warning = total_pct >= 80.0 and not overrun
154        info = not warning and not overrun
155
156        self._alert_error.set(overrun)
157        self._alert_warning.set(warning)
158        self._alert_info.set(info)
159
160        if overrun:
161            self._alert_error.setText(
162                f"Frame Timing: OVERRUN {total_pct:.0f}% ({total_ms:.1f}ms)"
163            )
164        elif warning:
165            self._alert_warning.setText(
166                f"Frame Timing: {total_pct:.0f}% - possible overrun"
167            )
168        else:
169            self._alert_info.setText(
170                f"Frame Timing: {total_pct:.0f}%"
171            )