nx_pid.nx source

1// nx_pid.nx -- PID controller (proportional-integral-derivative). 2// 3// u(t) = Kp*e + Ki*integral(e) + Kd*derivative(e) 4// 5// Closes the loop from Kalman state estimation to control actuation. 6// Pure i64 with Q14 gains. 7// 8// What this unlocks: 9// + temperature / motor / position control 10// + auto-stabilization (drone attitude, robot balance) 11// + feedback loops with arbitrary measurement source 12// + paired with Kalman: estimate state -> drive actuator 13// 14// Anti-windup: integral term is clamped to [-i_limit, +i_limit] so 15// long error sustains don't destroy the controller. 16// 17// genealogy_id: ziegler_nichols_1942 + minorsky_1922_first_pid 18// lineage_id: feedback_control_loop 19 20// nx_safety_envelope: 21// intended_use: AUTO_APPLIED -- primitive-specific tuning queued 22// sil_target: SIL1 23// evidence: [bulk_applied_2026-05-16, see-file-comment-for-detail] 24// verdict: NOT_YET_EVALUATED 25 26import "syscalls.nx" 27 28const NX_PID_Q: i64 = 16384 // Q14 29 30struct PID { 31 kp: i64, // Q14 proportional gain 32 ki: i64, // Q14 integral gain 33 kd: i64, // Q14 derivative gain 34 integral: i64, // accumulated error 35 prev_error: i64, 36 has_prev: i64, 37 i_limit: i64, // anti-windup clamp 38} 39 40func nx_pid_init(p: *PID, kp_q14: i64, ki_q14: i64, kd_q14: i64, 41 i_limit: i64) -> i64 { 42 p.kp = kp_q14 43 p.ki = ki_q14 44 p.kd = kd_q14 45 p.integral = 0 46 p.prev_error = 0 47 p.has_prev = 0 48 p.i_limit = i_limit 49 return 0 50} 51 52func nx_pid_reset(p: *PID) -> i64 { 53 p.integral = 0 54 p.prev_error = 0 55 p.has_prev = 0 56 return 0 57} 58 59// Compute control signal. setpoint = desired value, measured = current. 60// Returns control output (raw, not clamped; caller may clamp to actuator range). 61 62func nx_pid_update(p: *PID, setpoint: i64, measured: i64) -> i64 { 63 let error: i64 = setpoint - measured 64 65 // Integral with anti-windup clamp. 66 p.integral = p.integral + error 67 if p.integral > p.i_limit { p.integral = p.i_limit } 68 if p.integral < -p.i_limit { p.integral = -p.i_limit } 69 70 // Derivative (zero on first call). 71 var derivative: i64 = 0 72 if p.has_prev == 1 { 73 derivative = error - p.prev_error 74 } 75 p.prev_error = error 76 p.has_prev = 1 77 78 // u = Kp*e + Ki*integral + Kd*derivative, gains in Q14. 79 let p_term: i64 = (p.kp * error) / NX_PID_Q 80 let i_term: i64 = (p.ki * p.integral) / NX_PID_Q 81 let d_term: i64 = (p.kd * derivative) / NX_PID_Q 82 return p_term + i_term + d_term 83}