From 19a30a1c4f5f919086a49b86ee71f9dac4856495 Mon Sep 17 00:00:00 2001 From: sl-firmware Date: Sat, 7 Mar 2026 09:56:19 -0500 Subject: [PATCH] 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 --- include/jlink.h | 74 +++--- include/pid_flash.h | 48 ++++ .../config/autotune_config.yaml | 5 + .../pid_autotune_node.py | 221 ++++++++++++------ src/jlink.c | 30 +++ src/main.c | 28 +++ src/pid_flash.c | 72 ++++++ 7 files changed, 380 insertions(+), 98 deletions(-) create mode 100644 include/pid_flash.h create mode 100644 src/pid_flash.c diff --git a/include/jlink.h b/include/jlink.h index 3c70ff4..1b5b111 100644 --- a/include/jlink.h +++ b/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: - * 0x01 HEARTBEAT — no payload; refreshes heartbeat timer - * 0x02 DRIVE — int16 speed (-1000..+1000), int16 steer (-1000..+1000) - * 0x03 ARM — no payload; request arm (same interlock as CDC 'A') - * 0x04 DISARM — no payload; disarm immediately - * 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) - * 0x07 ESTOP — no payload; engage emergency stop + * Jetson to STM32 commands: + * 0x01 HEARTBEAT - no payload; refreshes heartbeat timer + * 0x02 DRIVE - int16 speed (-1000..+1000), int16 steer (-1000..+1000) + * 0x03 ARM - no payload; request arm (same interlock as CDC 'A') + * 0x04 DISARM - no payload; disarm immediately + * 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) + * 0x07 ESTOP - no payload; engage emergency stop + * 0x0A PID_SAVE - no payload; save current Kp/Ki/Kd to flash (Issue #531) * - * STM32 → Jetson telemetry: - * 0x80 STATUS — jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ + * STM32 to Jetson telemetry: + * 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 roll_x10; /* roll degrees ×10 */ - int16_t yaw_x10; /* yaw degrees ×10 (gyro-integrated) */ + int16_t pitch_x10; /* pitch degrees x10 */ + int16_t roll_x10; /* roll degrees x10 */ + 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 */ diff --git a/include/pid_flash.h b/include/pid_flash.h new file mode 100644 index 0000000..df0b9fb --- /dev/null +++ b/include/pid_flash.h @@ -0,0 +1,48 @@ +#ifndef PID_FLASH_H +#define PID_FLASH_H + +#include +#include + +/* + * 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 */ diff --git a/jetson/ros2_ws/src/saltybot_pid_autotune/config/autotune_config.yaml b/jetson/ros2_ws/src/saltybot_pid_autotune/config/autotune_config.yaml index 7eb2cb6..cb30d47 100644 --- a/jetson/ros2_ws/src/saltybot_pid_autotune/config/autotune_config.yaml +++ b/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 diff --git a/jetson/ros2_ws/src/saltybot_pid_autotune/saltybot_pid_autotune/pid_autotune_node.py b/jetson/ros2_ws/src/saltybot_pid_autotune/saltybot_pid_autotune/pid_autotune_node.py index cd5bd95..6e37885 100644 --- a/jetson/ros2_ws/src/saltybot_pid_autotune/saltybot_pid_autotune/pid_autotune_node.py +++ b/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("tilt_safety_limit", 25.0) # degrees - self.declare_parameter("max_tune_duration", 120.0) # seconds - self.declare_parameter("oscillation_count", 5) # Number of cycles to measure - self.declare_parameter("settle_time", 2.0) # Settle before starting relay + self.declare_parameter("relay_magnitude", 0.3) + self.declare_parameter("tilt_safety_limit", 25.0) + self.declare_parameter("max_tune_duration", 120.0) + self.declare_parameter("oscillation_count", 5) + 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.peaks: List[Tuple[float, float]] = [] # (time, peak_value) + self.feedback_history: List[Tuple[float, float]] = [] + 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 - - # During tuning, record history - if self.tune_state in (TuneState.MEASURING,): + if self.tune_state == 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 - kp, kd = self._compute_ziegler_nichols(ku, tu) + # Phase 4: Compute ZN gains + 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 - last_switch = oscillation_start + relay_state = True 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 - cmd = relay_on if relay_state else relay_off - self.pub_cmd_vel.publish(cmd) - - # Report progress + self.pub_cmd_vel.publish(relay_on if relay_state else relay_off) 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]]: - """Calculate Ku and Tu from measured peaks.""" + def _analyze_oscillation( + 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]: - """Compute Ziegler-Nichols PD gains. + def _compute_ziegler_nichols( + ku: float, tu: float + ) -> Tuple[float, float, float]: + """Ziegler-Nichols PID gains from relay feedback critical params. - For PD controller: - Kp = 0.6 * Ku - Kd = 0.075 * Ku * Tu (approximately Kp*Tu/8) + Standard ZN PID table: + Kp = 0.60 · Ku + 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(" 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(msg) + self.pub_info.publish(String(data=json.dumps(info))) def main(args=None): diff --git a/src/jlink.c b/src/jlink.c index dba55ec..86b1264 100644 --- a/src/jlink.c +++ b/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)); +} diff --git a/src/main.c b/src/main.c index 9fa3721..e9de09d 100644 --- a/src/main.c +++ b/src/main.c @@ -30,6 +30,7 @@ #include "battery.h" #include "coulomb_counter.h" #include "watchdog.h" +#include "pid_flash.h" #include #include #include @@ -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) || diff --git a/src/pid_flash.c b/src/pid_flash.c new file mode 100644 index 0000000..0131d62 --- /dev/null +++ b/src/pid_flash.c @@ -0,0 +1,72 @@ +#include "pid_flash.h" +#include "stm32f7xx_hal.h" +#include + +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, §or_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); +}