Add telemetry pipeline with REPL output for PID tuning (M2a pre-hardware)

Expose PID term contributions (last_p/last_i/last_d) for logging,
add TelemetryRecorder with pluggable sink (PrintSink now, SdSink when
Adalogger arrives), wire into main loop with configurable decimation.
Validated on hardware — CSV output confirmed via REPL.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

Author: c0ffee2code

decision/ADR-002-telemetry-logging.md

@@ -73,21 +73,21 @@ Separating the RTC onto its own bus avoids adding traffic to the sensor bus, whi
 
 ### Log format
 
-CSV with pipe delimiter for grep-friendliness (consistent with AS5600 diagnostic format):
+CSV with comma delimiter (standard CSV):
 
 ```
-T_RTC,T_MS,ENC_DEG,IMU_DEG,ERR,P,I,D,M1,M2
-2026-02-08T14:30:01,12345,+0.5,+0.8,-0.3,2.5,0.1,0.0,165,135
+T_MS,ENC_DEG,IMU_DEG,ERR,P,I,D,PID_OUT,M1,M2
+12345,+0.5,,+0.5,2.50,0.10,0.00,2.60,303,297
 ```
 
 Fields:
-- `T_RTC` — Wall clock from PCF8523 (ISO 8601, second resolution)
-- `T_MS` — `ticks_ms` since boot (sub-ms precision for inter-sample timing)
+- `T_MS` — `ticks_ms` since boot (sufficient without RTC; `T_RTC` column added as first column when Adalogger hardware arrives)
 - `ENC_DEG` — AS5600 angle in degrees (ground truth)
-- `IMU_DEG` — BNO085 angle in degrees (when available, empty otherwise)
+- `IMU_DEG` — BNO085 angle in degrees (empty until M2)
 - `ERR` — PID error term
 - `P`, `I`, `D` — Individual PID contributions
-- `M1`, `M2` — Motor throttle values
+- `PID_OUT` — Raw PID output before clamping to motor range (shows PID saturation)
+- `M1`, `M2` — Motor throttle values (integers)
 
 ### Write strategy
 
@@ -109,6 +109,19 @@ SD card root/
 
 Each file gets a header row on creation. RTC timestamp in the first data row provides the wall clock reference; subsequent rows use `T_MS` deltas for precise inter-sample timing.
 
+### Sampling & decoupling
+
+The telemetry pipeline separates data collection from I/O through a facade pattern:
+
+- **`TelemetryRecorder`** — facade called from the main loop. Accepts all telemetry fields per cycle, handles decimation, and delegates output to a pluggable sink.
+- **`PrintSink`** — current backend. Prints CSV rows to REPL serial console. No buffering needed.
+- **`SdSink`** (future) — buffers N rows in RAM, flushes to SD card file. Plug in when Adalogger hardware arrives — no structural changes to main loop or recorder.
+
+Configurable decimation via `TELEMETRY_SAMPLE_EVERY` constant in `main.py`:
+- Set high (e.g., 10000) for REPL output to avoid serial flood
+- Set to 1–5 for SD card logging to capture full-rate data
+- Decimation is cycle-count based: every N-th call to `record()` emits a row
+
 ## Consequences
 
 ### Positive

main.py

@@ -8,6 +8,7 @@
 from display_pack import (draw_disarmed, draw_arming, draw_ready,
                           draw_stabilizing, draw_error)
 from pid import PID
+from telemetry import TelemetryRecorder
 
 # =====================================================
 # Hardware
@@ -40,6 +41,9 @@
 # Display update (every N-th PID cycle to avoid display overhead each loop)
 DISPLAY_EVERY = const(5)  # 10 Hz display refresh
 
+# Telemetry decimation: 1=every cycle, N=every Nth (high for REPL, lower for SD)
+TELEMETRY_SAMPLE_EVERY = const(10)
+
 
 def clamp(value, lo, hi):
     if value < lo:
@@ -59,6 +63,7 @@ def buttons_by_held():
 # =====================================================
 def main():
     pid = PID(kp=5.0, ki=0.5, kd=0.0, integral_limit=200.0)
+    telemetry = TelemetryRecorder(TELEMETRY_SAMPLE_EVERY)
     motors = MotorThrottleGroup([MOTOR1_PIN, MOTOR2_PIN], DSHOT_SPEEDS.DSHOT600)
 
     try:
@@ -82,6 +87,7 @@ def main():
 
             # ----- STATE 4: STABILIZING -----
             pid.reset()
+            telemetry.begin_session()
             loop_count = 0
             prev_ms = utime.ticks_ms()
 
@@ -112,13 +118,20 @@ def main():
                 motors.setThrottle(0, m1)
                 motors.setThrottle(1, m2)
 
+                telemetry.record(
+                    now_ms, angle, None,
+                    angle, pid.last_p, pid.last_i, pid.last_d,
+                    output, m1, m2
+                )
+
                 # Display at reduced rate
                 loop_count += 1
                 if loop_count >= DISPLAY_EVERY:
                     loop_count = 0
                     draw_stabilizing(angle, m1, m2)
 
             # ----- DISARM -----
+            telemetry.end_session()
             motors.disarm()
             motors.stop()
 

pid.py

@@ -1,13 +1,20 @@
 class PID:
+    """Discrete PID controller with anti-windup and term introspection."""
+
     def __init__(self, kp, ki, kd=0.0, integral_limit=200.0):
+        """Configure gains and integral windup limit."""
         self.kp = kp
         self.ki = ki
         self.kd = kd
         self.integral_limit = integral_limit
         self._integral = 0.0
         self._prev_error = 0.0
+        self.last_p = 0.0
+        self.last_i = 0.0
+        self.last_d = 0.0
 
     def compute(self, error, dt):
+        """Return PID output for given error and timestep. Updates last_p/last_i/last_d."""
         self._integral += error * dt
         if self._integral > self.integral_limit:
             self._integral = self.integral_limit
@@ -17,8 +24,12 @@ def compute(self, error, dt):
         derivative = (error - self._prev_error) / dt if dt > 0 else 0.0
         self._prev_error = error
 
-        return self.kp * error + self.ki * self._integral + self.kd * derivative
+        self.last_p = self.kp * error
+        self.last_i = self.ki * self._integral
+        self.last_d = self.kd * derivative
+        return self.last_p + self.last_i + self.last_d
 
     def reset(self):
+        """Zero integrator and derivative state for a fresh control session."""
         self._integral = 0.0
         self._prev_error = 0.0

telemetry.py

@@ -0,0 +1,44 @@
+class PrintSink:
+    """Output backend that prints CSV rows to REPL serial console."""
+
+    def write(self, line):
+        """Emit a single CSV line to stdout."""
+        print(line)
+
+    def flush(self):
+        """No-op — stdout is unbuffered."""
+        pass
+
+
+class TelemetryRecorder:
+    """Facade that decimates and formats telemetry rows, delegating I/O to a sink."""
+
+    _HEADER = "T_MS,ENC_DEG,IMU_DEG,ERR,P,I,D,PID_OUT,M1,M2"
+
+    def __init__(self, sample_every, sink=None):
+        """Set decimation rate and output backend (defaults to PrintSink)."""
+        self._sample_every = sample_every
+        self._sink = sink or PrintSink()
+        self._counter = 0
+
+    def begin_session(self):
+        """Reset counter and emit CSV header. Call when entering STABILIZING state."""
+        self._counter = 0
+        self._sink.write(self._HEADER)
+
+    def record(self, t_ms, enc_deg, imu_deg, err, p, i, d, pid_out, m1, m2):
+        """Format and emit a CSV row every sample_every-th call. Others are silently dropped."""
+        self._counter += 1
+        if self._counter < self._sample_every:
+            return
+        self._counter = 0
+
+        imu_s = "" if imu_deg is None else "{:.2f}".format(imu_deg)
+        line = "{},{:.2f},{},{:.2f},{:.2f},{:.2f},{:.2f},{:.2f},{},{}".format(
+            t_ms, enc_deg, imu_s, err, p, i, d, pid_out, m1, m2
+        )
+        self._sink.write(line)
+
+    def end_session(self):
+        """Flush the sink. Call when leaving STABILIZING state (before disarm)."""
+        self._sink.flush()