feat: PID auto-tune for balance mode (Issue #531)

Implement Ziegler-Nichols relay feedback auto-tuning with flash persistence:

Firmware (STM32F722):
- pid_flash.c/h: erase+write Kp/Ki/Kd to flash sector 7 (0x0807FFC0),
  magic-validated; load on boot to restore saved tune
- jlink.h: add JLINK_CMD_PID_SAVE (0x0A) and JLINK_TLM_PID_RESULT (0x83)
  with jlink_tlm_pid_result_t struct and pid_save_req flag in JLinkState
- jlink.c: dispatch JLINK_CMD_PID_SAVE -> pid_save_req; add
  jlink_send_pid_result() to confirm flash write outcome over USART1
- main.c: load saved PID from flash after balance_init(); handle
  pid_save_req in main loop (disarmed-only, erase stalls CPU ~1s)

Jetson ROS2 (saltybot_pid_autotune):
- pid_autotune_node.py: add Ki to Ziegler-Nichols formula (ZN PID:
  Kp=0.6Ku, Ki=1.2Ku/Tu, Kd=0.075KuTu); add JLink serial client that
  sends JLINK_CMD_PID_SET + JLINK_CMD_PID_SAVE after tuning completes
- autotune_config.yaml: add jlink_serial_port and jlink_baud_rate params

Trigger: ros2 service call /saltybot/autotune_pid std_srvs/srv/Trigger

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

include/jlink.h

@@ -20,17 +20,20 @@
  *   CRC16 : CRC16-XModem over CMD+PAYLOAD (poly 0x1021, init 0), big-endian
  *   ETX   : frame end sentinel (0x03)
  *
- * Jetson → STM32 commands:
+ * Jetson to STM32 commands:
- *   0x01  HEARTBEAT  — no payload; refreshes heartbeat timer
+ *   0x01  HEARTBEAT  - no payload; refreshes heartbeat timer
- *   0x02  DRIVE      — int16 speed (-1000..+1000), int16 steer (-1000..+1000)
+ *   0x02  DRIVE      - int16 speed (-1000..+1000), int16 steer (-1000..+1000)
- *   0x03  ARM        — no payload; request arm (same interlock as CDC 'A')
+ *   0x03  ARM        - no payload; request arm (same interlock as CDC 'A')
- *   0x04  DISARM     — no payload; disarm immediately
+ *   0x04  DISARM     - no payload; disarm immediately
- *   0x05  PID_SET    — float kp, float ki, float kd (12 bytes, IEEE-754 LE)
+ *   0x05  PID_SET    - float kp, float ki, float kd (12 bytes, IEEE-754 LE)
- *   0x06  DFU_ENTER  — no payload; request OTA DFU reboot (denied while armed)
+ *   0x06  DFU_ENTER  - no payload; request OTA DFU reboot (denied while armed)
- *   0x07  ESTOP      — no payload; engage emergency stop
+ *   0x07  ESTOP      - no payload; engage emergency stop
+ *   0x0A  PID_SAVE   - no payload; save current Kp/Ki/Kd to flash (Issue #531)
  *
- * STM32 → Jetson telemetry:
+ * STM32 to Jetson telemetry:
- *   0x80  STATUS     — jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ
+ *   0x80  STATUS     - jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ
+ *   0x81  POWER      - jlink_tlm_power_t (11 bytes), sent at PM_TLM_HZ
+ *   0x83  PID_RESULT - jlink_tlm_pid_result_t (13 bytes), sent after PID_SAVE (Issue #531)
  *
  * Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
  * when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and
@@ -45,7 +48,7 @@
 #define JLINK_STX               0x02u
 #define JLINK_ETX               0x03u
 
-/* ---- Command IDs (Jetson → STM32) ---- */
+/* ---- Command IDs (Jetson to STM32) ---- */
 #define JLINK_CMD_HEARTBEAT     0x01u
 #define JLINK_CMD_DRIVE         0x02u
 #define JLINK_CMD_ARM           0x03u
@@ -55,16 +58,18 @@
 #define JLINK_CMD_ESTOP         0x07u
 #define JLINK_CMD_AUDIO         0x08u  /* PCM audio chunk: int16 samples, up to 126 */
 #define JLINK_CMD_SLEEP         0x09u  /* no payload; request STOP-mode sleep */
+#define JLINK_CMD_PID_SAVE      0x0Au  /* no payload; save Kp/Ki/Kd to flash (Issue #531) */
 
-/* ---- Telemetry IDs (STM32 → Jetson) ---- */
+/* ---- Telemetry IDs (STM32 to Jetson) ---- */
 #define JLINK_TLM_STATUS        0x80u
 #define JLINK_TLM_POWER         0x81u  /* jlink_tlm_power_t (11 bytes) */
+#define JLINK_TLM_PID_RESULT    0x83u  /* jlink_tlm_pid_result_t (13 bytes) Issue #531 */
 
 /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
 typedef struct __attribute__((packed)) {
-    int16_t  pitch_x10;     /* pitch degrees ×10 */
+    int16_t  pitch_x10;     /* pitch degrees x10 */
-    int16_t  roll_x10;      /* roll  degrees ×10 */
+    int16_t  roll_x10;      /* roll  degrees x10 */
-    int16_t  yaw_x10;       /* yaw   degrees ×10 (gyro-integrated) */
+    int16_t  yaw_x10;       /* yaw   degrees x10 (gyro-integrated) */
     int16_t  motor_cmd;     /* ESC output -1000..+1000 */
     uint16_t vbat_mv;       /* battery millivolts */
     int8_t   rssi_dbm;      /* CRSF RSSI (dBm, negative) */
@@ -88,30 +93,41 @@ typedef struct __attribute__((packed)) {
     uint32_t idle_ms;       /* ms since last cmd_vel activity */
 } jlink_tlm_power_t;  /* 11 bytes */
 
+/* ---- Telemetry PID_RESULT payload (13 bytes, packed) Issue #531 ---- */
+/* Sent after JLINK_CMD_PID_SAVE is processed; confirms gains written to flash. */
+typedef struct __attribute__((packed)) {
+    float   kp;        /* Kp saved */
+    float   ki;        /* Ki saved */
+    float   kd;        /* Kd saved */
+    uint8_t saved_ok;  /* 1 = flash write verified, 0 = write failed */
+} jlink_tlm_pid_result_t;  /* 13 bytes */
+
 /* ---- Volatile state (read from main loop) ---- */
 typedef struct {
-    /* Drive command — updated on JLINK_CMD_DRIVE */
+    /* Drive command - updated on JLINK_CMD_DRIVE */
     volatile int16_t  speed;        /* -1000..+1000 */
     volatile int16_t  steer;        /* -1000..+1000 */
 
-    /* Heartbeat timer — updated on any valid frame */
+    /* Heartbeat timer - updated on any valid frame */
     volatile uint32_t last_rx_ms;   /* HAL_GetTick() of last valid frame; 0=none */
 
-    /* One-shot request flags — set by parser, cleared by main loop */
+    /* One-shot request flags - set by parser, cleared by main loop */
     volatile uint8_t  arm_req;
     volatile uint8_t  disarm_req;
     volatile uint8_t  estop_req;
 
-    /* PID update — set by parser, cleared by main loop */
+    /* PID update - set by parser, cleared by main loop */
     volatile uint8_t  pid_updated;
     volatile float    pid_kp;
     volatile float    pid_ki;
     volatile float    pid_kd;
 
-    /* DFU reboot request — set by parser, cleared by main loop */
+    /* DFU reboot request - set by parser, cleared by main loop */
     volatile uint8_t  dfu_req;
-    /* Sleep request — set by JLINK_CMD_SLEEP, cleared by main loop */
+    /* Sleep request - set by JLINK_CMD_SLEEP, cleared by main loop */
     volatile uint8_t  sleep_req;
+    /* PID save request - set by JLINK_CMD_PID_SAVE, cleared by main loop (Issue #531) */
+    volatile uint8_t  pid_save_req;
 } JLinkState;
 
 extern volatile JLinkState jlink_state;
@@ -119,34 +135,40 @@ extern volatile JLinkState jlink_state;
 /* ---- API ---- */
 
 /*
- * jlink_init() — configure USART1 (PB6=TX, PB7=RX) at 921600 baud with
+ * jlink_init() - configure USART1 (PB6=TX, PB7=RX) at 921600 baud with
  * DMA2_Stream2_Channel4 circular RX (128-byte buffer) and IDLE interrupt.
  * Call once before safety_init().
  */
 void jlink_init(void);
 
 /*
- * jlink_is_active(now_ms) — returns true if a valid frame arrived within
+ * jlink_is_active(now_ms) - returns true if a valid frame arrived within
  * JLINK_HB_TIMEOUT_MS. Returns false if no frame ever received.
  */
 bool jlink_is_active(uint32_t now_ms);
 
 /*
- * jlink_send_telemetry(status) — build and transmit a JLINK_TLM_STATUS frame
+ * jlink_send_telemetry(status) - build and transmit a JLINK_TLM_STATUS frame
  * over USART1 TX (blocking, ~0.2ms at 921600). Call at JLINK_TLM_HZ.
  */
 void jlink_send_telemetry(const jlink_tlm_status_t *status);
 
 /*
- * jlink_process() — drain DMA circular buffer and parse frames.
+ * jlink_process() - drain DMA circular buffer and parse frames.
  * Call from main loop every iteration (not ISR). Lightweight: O(bytes_pending).
  */
 void jlink_process(void);
 
 /*
- * jlink_send_power_telemetry(power) — build and transmit a JLINK_TLM_POWER
+ * jlink_send_power_telemetry(power) - build and transmit a JLINK_TLM_POWER
  * frame (17 bytes) at PM_TLM_HZ. Call from main loop when not in STOP mode.
  */
 void jlink_send_power_telemetry(const jlink_tlm_power_t *power);
 
+/*
+ * jlink_send_pid_result(result) - build and transmit a JLINK_TLM_PID_RESULT
+ * frame (19 bytes) to confirm PID flash save outcome (Issue #531).
+ */
+void jlink_send_pid_result(const jlink_tlm_pid_result_t *result);
+
 #endif /* JLINK_H */

include/pid_flash.h

@@ -0,0 +1,48 @@
+#ifndef PID_FLASH_H
+#define PID_FLASH_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * pid_flash — persistent PID storage for Issue #531 (auto-tune).
+ *
+ * Stores Kp, Ki, Kd in the last 64 bytes of STM32F722 flash sector 7
+ * (0x0807FFC0). Magic word validates presence of saved params.
+ * Sector 7 is 128KB starting at 0x08060000; firmware never exceeds sector 6.
+ *
+ * Flash writes require an erase of the full sector (128KB) before re-writing.
+ * The store address is the very last 64-byte block so future expansion can
+ * grow toward lower addresses within sector 7 without conflict.
+ */
+
+#define PID_FLASH_SECTOR        FLASH_SECTOR_7
+#define PID_FLASH_SECTOR_VOLTAGE VOLTAGE_RANGE_3   /* 2.7V-3.6V, 32-bit parallelism */
+
+/* Sector 7: 128KB at 0x08060000; store in last 64 bytes */
+#define PID_FLASH_STORE_ADDR    0x0807FFC0UL
+#define PID_FLASH_MAGIC         0x534C5401UL   /* 'SLT\x01' — version 1 */
+
+typedef struct __attribute__((packed)) {
+    uint32_t magic;       /* PID_FLASH_MAGIC when valid */
+    float    kp;
+    float    ki;
+    float    kd;
+    uint8_t  _pad[48];   /* padding to 64 bytes */
+} pid_flash_t;
+
+/*
+ * pid_flash_load() — read saved PID from flash.
+ * Returns true and fills *kp/*ki/*kd if magic is valid.
+ * Returns false if no valid params stored (caller keeps defaults).
+ */
+bool pid_flash_load(float *kp, float *ki, float *kd);
+
+/*
+ * pid_flash_save() — erase sector 7 and write Kp/Ki/Kd.
+ * Must not be called while armed (flash erase takes ~1s and stalls the CPU).
+ * Returns true on success.
+ */
+bool pid_flash_save(float kp, float ki, float kd);
+
+#endif /* PID_FLASH_H */

jetson/ros2_ws/src/saltybot_pid_autotune/config/autotune_config.yaml

@@ -16,3 +16,8 @@ pid_autotune:
 
     # Time to allow system to settle before starting relay (seconds)
     settle_time: 2.0
+
+    # JLink serial port for sending computed gains to FC (Issue #531)
+    # USART1 on FC maps to this ttyTHS device on Jetson Orin
+    jlink_serial_port: "/dev/ttyTHS0"
+    jlink_baud_rate: 921600

jetson/ros2_ws/src/saltybot_pid_autotune/saltybot_pid_autotune/pid_autotune_node.py

@@ -2,7 +2,9 @@
 """PID auto-tuner for SaltyBot using Astrom-Hagglund relay feedback method.
 
 Service-triggered relay oscillation measures ultimate gain (Ku) and ultimate period
-(Tu), then computes Ziegler-Nichols PD gains. Aborts on >25deg tilt for safety.
+(Tu), then computes Ziegler-Nichols PID gains (Kp, Ki, Kd). After tuning the computed
+gains are written to the FC via JLink binary protocol (JLINK_CMD_PID_SET) and
+immediately saved to flash (JLINK_CMD_PID_SAVE). Aborts on >25deg tilt for safety.
 
 Service:
   /saltybot/autotune_pid (std_srvs/Trigger) - Start auto-tuning process
@@ -18,18 +20,53 @@ Subscribed topics:
 
 import json
 import math
+import struct
 import time
 from enum import Enum
 from typing import Optional, Tuple, List
 
 import rclpy
 from rclpy.node import Node
-from rclpy.service import Service
 from geometry_msgs.msg import Twist
 from sensor_msgs.msg import Imu
 from std_msgs.msg import Float32, String
 from std_srvs.srv import Trigger
 
+try:
+    import serial
+    _SERIAL_AVAILABLE = True
+except ImportError:
+    _SERIAL_AVAILABLE = False
+
+
+# ── JLink binary protocol constants (must match jlink.h) ─────────────────────
+JLINK_STX = 0x02
+JLINK_ETX = 0x03
+JLINK_CMD_PID_SET  = 0x05
+JLINK_CMD_PID_SAVE = 0x0A
+
+
+def _crc16_xmodem(data: bytes) -> int:
+    """CRC16-XModem (poly 0x1021, init 0x0000) — matches FC implementation."""
+    crc = 0x0000
+    for byte in data:
+        crc ^= byte << 8
+        for _ in range(8):
+            if crc & 0x8000:
+                crc = (crc << 1) ^ 0x1021
+            else:
+                crc <<= 1
+        crc &= 0xFFFF
+    return crc
+
+
+def _build_jlink_frame(cmd: int, payload: bytes) -> bytes:
+    """Build a JLink binary frame: [STX][LEN][CMD][PAYLOAD][CRC_hi][CRC_lo][ETX]."""
+    data = bytes([cmd]) + payload
+    length = len(data)
+    crc = _crc16_xmodem(data)
+    return bytes([JLINK_STX, length]) + data + bytes([crc >> 8, crc & 0xFF, JLINK_ETX])
+
 
 class TuneState(Enum):
     """State machine for auto-tuning process."""
@@ -47,24 +84,28 @@ class PIDAutotuneNode(Node):
         super().__init__("pid_autotune")
 
         # Parameters
-        self.declare_parameter("relay_magnitude", 0.3)  # Relay on-off magnitude
+        self.declare_parameter("relay_magnitude", 0.3)
-        self.declare_parameter("tilt_safety_limit", 25.0)  # degrees
+        self.declare_parameter("tilt_safety_limit", 25.0)
-        self.declare_parameter("max_tune_duration", 120.0)  # seconds
+        self.declare_parameter("max_tune_duration", 120.0)
-        self.declare_parameter("oscillation_count", 5)  # Number of cycles to measure
+        self.declare_parameter("oscillation_count", 5)
-        self.declare_parameter("settle_time", 2.0)  # Settle before starting relay
+        self.declare_parameter("settle_time", 2.0)
+        self.declare_parameter("jlink_serial_port", "/dev/ttyTHS0")
+        self.declare_parameter("jlink_baud_rate", 921600)
 
-        self.relay_magnitude = self.get_parameter("relay_magnitude").value
+        self.relay_magnitude   = self.get_parameter("relay_magnitude").value
         self.tilt_safety_limit = self.get_parameter("tilt_safety_limit").value
         self.max_tune_duration = self.get_parameter("max_tune_duration").value
         self.oscillation_count = self.get_parameter("oscillation_count").value
-        self.settle_time = self.get_parameter("settle_time").value
+        self.settle_time       = self.get_parameter("settle_time").value
+        self._jlink_port       = self.get_parameter("jlink_serial_port").value
+        self._jlink_baud       = self.get_parameter("jlink_baud_rate").value
 
-        # State tracking
+        # State
         self.tune_state = TuneState.IDLE
         self.last_imu_tilt = 0.0
         self.last_feedback = 0.0
-        self.feedback_history: List[Tuple[float, float]] = []  # (time, feedback)
+        self.feedback_history: List[Tuple[float, float]] = []
-        self.peaks: List[Tuple[float, float]] = []  # (time, peak_value)
+        self.peaks: List[Tuple[float, float]] = []
         self.start_time = None
 
         # Subscriptions
@@ -74,7 +115,9 @@ class PIDAutotuneNode(Node):
         )
 
         # Publications
-        self.pub_cmd_vel = self.create_publisher(Twist, "/saltybot/autotune_cmd_vel", 10)
+        self.pub_cmd_vel = self.create_publisher(
+            Twist, "/saltybot/autotune_cmd_vel", 10
+        )
         self.pub_info = self.create_publisher(String, "/saltybot/autotune_info", 10)
 
         # Service
@@ -86,31 +129,29 @@ class PIDAutotuneNode(Node):
             "PID auto-tuner initialized. Service: /saltybot/autotune_pid\n"
             f"Relay magnitude: {self.relay_magnitude}, "
             f"Tilt safety limit: {self.tilt_safety_limit}°, "
-            f"Max duration: {self.max_tune_duration}s"
+            f"Max duration: {self.max_tune_duration}s, "
+            f"JLink port: {self._jlink_port}"
         )
 
+    # ── IMU / feedback ────────────────────────────────────────────────────────
+
     def _on_imu(self, msg: Imu) -> None:
-        """Extract tilt angle from IMU."""
-        # Approximate tilt from acceleration (pitch angle)
-        # pitch = atan2(ax, sqrt(ay^2 + az^2))
         accel = msg.linear_acceleration
         self.last_imu_tilt = math.degrees(
             math.atan2(accel.x, math.sqrt(accel.y**2 + accel.z**2))
         )
 
     def _on_feedback(self, msg: Float32) -> None:
-        """Record balance feedback for oscillation measurement."""
         now = self.get_clock().now().nanoseconds / 1e9
         self.last_feedback = msg.data
-
+        if self.tune_state == TuneState.MEASURING:
-        # During tuning, record history
-        if self.tune_state in (TuneState.MEASURING,):
             self.feedback_history.append((now, msg.data))
 
+    # ── Service handler ───────────────────────────────────────────────────────
+
     def _handle_autotune_request(
         self, request: Trigger.Request, response: Trigger.Response
     ) -> Trigger.Response:
-        """Handle service request to start auto-tuning."""
         if self.tune_state != TuneState.IDLE:
             response.success = False
             response.message = f"Already tuning (state: {self.tune_state.name})"
@@ -122,15 +163,15 @@ class PIDAutotuneNode(Node):
         self.peaks = []
         self.start_time = self.get_clock().now().nanoseconds / 1e9
 
-        # Start tuning process
         self._run_autotuning()
 
         response.success = self.tune_state == TuneState.COMPLETE
         response.message = f"Tuning {self.tune_state.name}"
         return response
 
+    # ── Tuning sequence ───────────────────────────────────────────────────────
+
     def _run_autotuning(self) -> None:
-        """Execute the complete auto-tuning sequence."""
         # Phase 1: Settle
         self.get_logger().info("Phase 1: Settling...")
         if not self._settle():
@@ -150,7 +191,7 @@ class PIDAutotuneNode(Node):
             )
             return
 
-        # Phase 3: Analysis
+        # Phase 3: Analyse
         self.get_logger().info("Phase 3: Analyzing measurements...")
         ku, tu = self._analyze_oscillation()
         if ku is None or tu is None:
@@ -160,10 +201,12 @@ class PIDAutotuneNode(Node):
             )
             return
 
-        # Phase 4: Compute gains
+        # Phase 4: Compute ZN gains
-        kp, kd = self._compute_ziegler_nichols(ku, tu)
+        kp, ki, kd = self._compute_ziegler_nichols(ku, tu)
+
+        # Phase 5: Push gains to FC and save to flash
+        saved = self._send_pid_to_fc(kp, ki, kd)
 
-        # Done
         self.tune_state = TuneState.COMPLETE
         self._publish_info(
             {
@@ -171,35 +214,33 @@ class PIDAutotuneNode(Node):
                 "ku": ku,
                 "tu": tu,
                 "kp": kp,
+                "ki": ki,
                 "kd": kd,
+                "flash_saved": saved,
             }
         )
         self.get_logger().info(
-            f"Auto-tune complete: Ku={ku:.4f}, Tu={tu:.2f}s, Kp={kp:.4f}, Kd={kd:.4f}"
+            f"Auto-tune complete: Ku={ku:.4f}, Tu={tu:.2f}s | "
+            f"Kp={kp:.4f}, Ki={ki:.4f}, Kd={kd:.4f} | flash_saved={saved}"
         )
 
     def _settle(self) -> bool:
-        """Allow system to settle before starting relay."""
         settle_start = self.get_clock().now().nanoseconds / 1e9
-        relay_off = Twist()  # Zero command
+        relay_off = Twist()
 
         while True:
             now = self.get_clock().now().nanoseconds / 1e9
             elapsed = now - settle_start
 
-            # Safety check
             if abs(self.last_imu_tilt) > self.tilt_safety_limit:
                 self.get_logger().error(
-                    f"Safety abort: Tilt {self.last_imu_tilt:.1f}° > {self.tilt_safety_limit}°"
+                    f"Safety abort: Tilt {self.last_imu_tilt:.1f}° > "
+                    f"{self.tilt_safety_limit}°"
                 )
                 return False
-
-            # Timeout check
             if elapsed > self.max_tune_duration:
                 self.get_logger().error("Settling timeout")
                 return False
-
-            # Continue settling
             if elapsed > self.settle_time:
                 return True
 
@@ -207,57 +248,47 @@ class PIDAutotuneNode(Node):
             time.sleep(0.01)
 
     def _relay_oscillation(self) -> bool:
-        """Apply relay control to induce oscillation."""
         relay_on = Twist()
         relay_on.linear.x = self.relay_magnitude
-
         relay_off = Twist()
 
         oscillation_start = self.get_clock().now().nanoseconds / 1e9
-        relay_state = True  # True = relay ON, False = relay OFF
+        relay_state = True
-        last_switch = oscillation_start
         cycles_measured = 0
-        last_peak_value = None
 
         while cycles_measured < self.oscillation_count:
             now = self.get_clock().now().nanoseconds / 1e9
             elapsed = now - oscillation_start
 
-            # Safety checks
             if abs(self.last_imu_tilt) > self.tilt_safety_limit:
                 self.get_logger().error(
-                    f"Safety abort: Tilt {self.last_imu_tilt:.1f}° > {self.tilt_safety_limit}°"
+                    f"Safety abort: Tilt {self.last_imu_tilt:.1f}° > "
+                    f"{self.tilt_safety_limit}°"
                 )
                 return False
-
             if elapsed > self.max_tune_duration:
                 self.get_logger().error("Relay oscillation timeout")
                 return False
 
-            # Switch relay at zero crossings
+            # Switch relay at zero crossings of balance feedback
             if relay_state and self.last_feedback < 0:
                 relay_state = False
                 self.peaks.append((now, self.last_feedback))
                 cycles_measured += 1
-                last_switch = now
                 self.get_logger().debug(
-                    f"Cycle {cycles_measured}: Peak at {now - oscillation_start:.2f}s = {self.last_feedback:.4f}"
+                    f"Cycle {cycles_measured}: peak={self.last_feedback:.4f} "
+                    f"t={elapsed:.2f}s"
                 )
-
             elif not relay_state and self.last_feedback > 0:
                 relay_state = True
                 self.peaks.append((now, self.last_feedback))
                 cycles_measured += 1
-                last_switch = now
                 self.get_logger().debug(
-                    f"Cycle {cycles_measured}: Peak at {now - oscillation_start:.2f}s = {self.last_feedback:.4f}"
+                    f"Cycle {cycles_measured}: peak={self.last_feedback:.4f} "
+                    f"t={elapsed:.2f}s"
                 )
 
-            # Publish relay command
+            self.pub_cmd_vel.publish(relay_on if relay_state else relay_off)
-            cmd = relay_on if relay_state else relay_off
-            self.pub_cmd_vel.publish(cmd)
-
-            # Report progress
             self._publish_info(
                 {
                     "status": "measuring",
@@ -266,25 +297,24 @@ class PIDAutotuneNode(Node):
                     "tilt": self.last_imu_tilt,
                 }
             )
-
             time.sleep(0.01)
 
-        # Stop relay
         self.pub_cmd_vel.publish(relay_off)
         return len(self.peaks) >= self.oscillation_count
 
-    def _analyze_oscillation(self) -> Tuple[Optional[float], Optional[float]]:
+    def _analyze_oscillation(
-        """Calculate Ku and Tu from measured peaks."""
+        self,
+    ) -> Tuple[Optional[float], Optional[float]]:
         if len(self.peaks) < 3:
             self.get_logger().error(f"Insufficient peaks: {len(self.peaks)}")
             return None, None
 
-        # Ku = 4*relay_magnitude / (pi * |peak|)
+        # Ku = 4·d / (π·|a|)  where d = relay magnitude, a = average peak amplitude
         peak_values = [abs(p[1]) for p in self.peaks]
         avg_peak = sum(peak_values) / len(peak_values)
         ku = (4.0 * self.relay_magnitude) / (math.pi * avg_peak)
 
-        # Tu = 2 * (time between peaks) - average period
+        # Tu = 2 × mean half-period (time between consecutive zero crossings)
         peak_times = [p[0] for p in self.peaks]
         time_diffs = [
             peak_times[i + 1] - peak_times[i] for i in range(len(peak_times) - 1)
@@ -297,26 +327,73 @@ class PIDAutotuneNode(Node):
         return ku, tu
 
     @staticmethod
-    def _compute_ziegler_nichols(ku: float, tu: float) -> Tuple[float, float]:
+    def _compute_ziegler_nichols(
-        """Compute Ziegler-Nichols PD gains.
+        ku: float, tu: float
+    ) -> Tuple[float, float, float]:
+        """Ziegler-Nichols PID gains from relay feedback critical params.
 
-        For PD controller:
+        Standard ZN PID table:
-          Kp = 0.6 * Ku
+          Kp = 0.60 · Ku
-          Kd = 0.075 * Ku * Tu (approximately Kp*Tu/8)
+          Ki = 1.20 · Ku / Tu     (= 2·Kp / Tu)
+          Kd = 0.075 · Ku · Tu    (= Kp·Tu / 8)
         """
-        kp = 0.6 * ku
+        kp = 0.60 * ku
+        ki = 1.20 * ku / tu
         kd = 0.075 * ku * tu
-        return kp, kd
+        return kp, ki, kd
+
+    # ── JLink communication ───────────────────────────────────────────────────
+
+    def _send_pid_to_fc(self, kp: float, ki: float, kd: float) -> bool:
+        """Send JLINK_CMD_PID_SET then JLINK_CMD_PID_SAVE over JLink serial.
+
+        Returns True if both frames were written without serial error.
+        """
+        if not _SERIAL_AVAILABLE:
+            self.get_logger().warn(
+                "pyserial not available — PID gains not sent to FC. "
+                "Install with: pip install pyserial"
+            )
+            return False
+
+        try:
+            payload_set = struct.pack("<fff", kp, ki, kd)  # 12 bytes, little-endian
+            frame_set  = _build_jlink_frame(JLINK_CMD_PID_SET, payload_set)
+            frame_save = _build_jlink_frame(JLINK_CMD_PID_SAVE, b"")
+
+            with serial.Serial(
+                port=self._jlink_port,
+                baudrate=self._jlink_baud,
+                timeout=1.0,
+                write_timeout=0.5,
+            ) as ser:
+                ser.write(frame_set)
+                time.sleep(0.05)   # allow FC to process PID_SET
+                ser.write(frame_save)
+                # Flash erase takes ~1s on STM32F7; wait for it
+                time.sleep(1.5)
+
+            self.get_logger().info(
+                f"Sent PID_SET+PID_SAVE to FC via {self._jlink_port}: "
+                f"Kp={kp:.4f}, Ki={ki:.4f}, Kd={kd:.4f}"
+            )
+            return True
+
+        except Exception as exc:
+            self.get_logger().error(
+                f"Failed to send PID to FC on {self._jlink_port}: {exc}"
+            )
+            return False
+
+    # ── Helpers ───────────────────────────────────────────────────────────────
 
     def _publish_info(self, data: dict) -> None:
-        """Publish tuning information as JSON."""
         info = {
             "timestamp": self.get_clock().now().nanoseconds / 1e9,
             "state": self.tune_state.name,
             **data,
         }
-        msg = String(data=json.dumps(info))
+        self.pub_info.publish(String(data=json.dumps(info)))
-        self.pub_info.publish(msg)
 
 
 def main(args=None):

src/jlink.c

@@ -203,6 +203,11 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
         jlink_state.sleep_req = 1u;
         break;
 
+    case JLINK_CMD_PID_SAVE:
+        /* Payload-less; main loop calls pid_flash_save() (Issue #531) */
+        jlink_state.pid_save_req = 1u;
+        break;
+
     default:
         break;
     }
@@ -342,3 +347,28 @@ void jlink_send_power_telemetry(const jlink_tlm_power_t *power)
 
     jlink_tx_locked(frame, sizeof(frame));
 }
+
+/* ---- jlink_send_pid_result() -- Issue #531 ---- */
+void jlink_send_pid_result(const jlink_tlm_pid_result_t *result)
+{
+    /*
+     * Frame: [STX][LEN][0x83][13 bytes PID_RESULT][CRC_hi][CRC_lo][ETX]
+     * LEN = 1 (CMD) + 13 (payload) = 14; total frame = 19 bytes.
+     * At 921600 baud: 19x10/921600 ~0.21 ms -- safe to block.
+     */
+    static uint8_t frame_pid[19];
+    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_pid_result_t);  /* 13 */
+    const uint8_t  flen = 1u + plen;                                 /* 14 */
+
+    frame_pid[0] = JLINK_STX;
+    frame_pid[1] = flen;
+    frame_pid[2] = JLINK_TLM_PID_RESULT;
+    memcpy(&frame_pid[3], result, plen);
+
+    uint16_t crc_pid = crc16_xmodem(&frame_pid[2], flen);
+    frame_pid[3 + plen]     = (uint8_t)(crc_pid >> 8);
+    frame_pid[3 + plen + 1] = (uint8_t)(crc_pid & 0xFFu);
+    frame_pid[3 + plen + 2] = JLINK_ETX;
+
+    jlink_tx_locked(frame_pid, sizeof(frame_pid));
+}

src/main.c

@@ -30,6 +30,7 @@
 #include "battery.h"
 #include "coulomb_counter.h"
 #include "watchdog.h"
+#include "pid_flash.h"
 #include <math.h>
 #include <string.h>
 #include <stdio.h>
@@ -163,6 +164,18 @@ int main(void) {
     balance_t bal;
     balance_init(&bal);
 
+    /* Load PID from flash if previously auto-tuned (Issue #531) */
+    {
+        float flash_kp, flash_ki, flash_kd;
+        if (pid_flash_load(&flash_kp, &flash_ki, &flash_kd)) {
+            bal.kp = flash_kp;
+            bal.ki = flash_ki;
+            bal.kd = flash_kd;
+            printf("[PID] Loaded from flash: Kp=%.3f Ki=%.3f Kd=%.3f\n",
+                   (double)flash_kp, (double)flash_ki, (double)flash_kd);
+        }
+    }
+
     /* Init motor driver */
     motor_driver_t motors;
     motor_driver_init(&motors);
@@ -334,6 +347,21 @@ int main(void) {
             jlink_state.sleep_req = 0u;
             power_mgmt_request_sleep();
         }
+        /* PID_SAVE: persist current gains to flash, reply with result (Issue #531).
+         * Only allowed while disarmed -- flash sector erase stalls CPU for ~1 s. */
+        if (jlink_state.pid_save_req && bal.state != BALANCE_ARMED) {
+            jlink_state.pid_save_req = 0u;
+            bool save_ok = pid_flash_save(bal.kp, bal.ki, bal.kd);
+            jlink_tlm_pid_result_t pid_res;
+            pid_res.kp       = bal.kp;
+            pid_res.ki       = bal.ki;
+            pid_res.kd       = bal.kd;
+            pid_res.saved_ok = save_ok ? 1u : 0u;
+            jlink_send_pid_result(&pid_res);
+            printf("[PID] Flash save %s: Kp=%.3f Ki=%.3f Kd=%.3f\n",
+                   save_ok ? "OK" : "FAIL",
+                   (double)bal.kp, (double)bal.ki, (double)bal.kd);
+        }
 
         /* Power management: CRSF/JLink activity or armed state resets idle timer */
         if ((crsf_state.last_rx_ms != 0 && (now - crsf_state.last_rx_ms) < 500) ||

src/pid_flash.c

@@ -0,0 +1,72 @@
+#include "pid_flash.h"
+#include "stm32f7xx_hal.h"
+#include <string.h>
+
+bool pid_flash_load(float *kp, float *ki, float *kd)
+{
+    const pid_flash_t *p = (const pid_flash_t *)PID_FLASH_STORE_ADDR;
+
+    if (p->magic != PID_FLASH_MAGIC) return false;
+
+    /* Basic sanity bounds — same as JLINK_CMD_PID_SET handler */
+    if (p->kp < 0.0f || p->kp > 500.0f) return false;
+    if (p->ki < 0.0f || p->ki > 50.0f)  return false;
+    if (p->kd < 0.0f || p->kd > 50.0f)  return false;
+
+    *kp = p->kp;
+    *ki = p->ki;
+    *kd = p->kd;
+    return true;
+}
+
+bool pid_flash_save(float kp, float ki, float kd)
+{
+    HAL_StatusTypeDef rc;
+
+    /* Unlock flash */
+    rc = HAL_FLASH_Unlock();
+    if (rc != HAL_OK) return false;
+
+    /* Erase sector 7 */
+    FLASH_EraseInitTypeDef erase = {
+        .TypeErase    = FLASH_TYPEERASE_SECTORS,
+        .Sector       = PID_FLASH_SECTOR,
+        .NbSectors    = 1,
+        .VoltageRange = PID_FLASH_SECTOR_VOLTAGE,
+    };
+    uint32_t sector_error = 0;
+    rc = HAL_FLASHEx_Erase(&erase, &sector_error);
+    if (rc != HAL_OK || sector_error != 0xFFFFFFFFUL) {
+        HAL_FLASH_Lock();
+        return false;
+    }
+
+    /* Build record */
+    pid_flash_t rec;
+    memset(&rec, 0xFF, sizeof(rec));
+    rec.magic = PID_FLASH_MAGIC;
+    rec.kp    = kp;
+    rec.ki    = ki;
+    rec.kd    = kd;
+
+    /* Write 64 bytes as 16 × 32-bit words */
+    const uint32_t *src  = (const uint32_t *)&rec;
+    uint32_t        addr = PID_FLASH_STORE_ADDR;
+    for (uint8_t i = 0; i < sizeof(rec) / 4u; i++) {
+        rc = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, addr, src[i]);
+        if (rc != HAL_OK) {
+            HAL_FLASH_Lock();
+            return false;
+        }
+        addr += 4u;
+    }
+
+    HAL_FLASH_Lock();
+
+    /* Verify readback */
+    const pid_flash_t *stored = (const pid_flash_t *)PID_FLASH_STORE_ADDR;
+    return (stored->magic == PID_FLASH_MAGIC &&
+            stored->kp == kp &&
+            stored->ki == ki &&
+            stored->kd == kd);
+}