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>
This commit is contained in:
parent
e67783f313
commit
19a30a1c4f
@ -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 */
|
||||
|
||||
48
include/pid_flash.h
Normal file
48
include/pid_flash.h
Normal file
@ -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 */
|
||||
@ -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
|
||||
|
||||
@ -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.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._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("<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(msg)
|
||||
self.pub_info.publish(String(data=json.dumps(info)))
|
||||
|
||||
|
||||
def main(args=None):
|
||||
|
||||
30
src/jlink.c
30
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));
|
||||
}
|
||||
|
||||
28
src/main.c
28
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) ||
|
||||
|
||||
72
src/pid_flash.c
Normal file
72
src/pid_flash.c
Normal file
@ -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, §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);
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user