Files
hpsg5-controller_v2-stm32g4/Core/Advance_Control/pwm.h

388 lines
21 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
/**
* @file pwm.h (families/t06211/compact_src)
* @brief Compact single-header API for the t06211 PWM controller.
*
* Mirrors the public shape of the default family's `compact_src/pwm.h`
* but tailored to t06211's 5-stage pipeline (no standalone max_cl_error
* lookup, single-submap setpoint, factor+offset target compute).
*
* Public API is just two functions:
* pwm_init() — one-time setup
* pwm_service() — per-cycle update (pulls inputs via getters,
* runs the 5-stage pipeline, writes rt outputs)
*
* External inputs arrive through a getter vtable; each callback returns
* one signal in native PWM units.
*
* Every arithmetic choice mirrors the MCS-96 assembly; see
* families/t06211/src/ for per-stage commentary.
*/
#ifndef PWM_H
#define PWM_H
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
/* ── MCS-96 arithmetic primitives ───────────────────────────────────── */
#define MUL_S16(a, b) ((int32_t)(int16_t)(a) * (int32_t)(int16_t)(b))
#define MULU_U16(a, b) ((uint32_t)(uint16_t)(a) * (uint32_t)(uint16_t)(b))
#define CLAMP(v, lo, hi) ((v) < (lo) ? (lo) : (v) > (hi) ? (hi) : (v))
static inline int16_t shra16(int16_t v, unsigned n) {
if (!n) return v;
uint16_t u = (uint16_t)v, sign = (u >> 15) & 1u;
uint16_t m = sign ? (uint16_t)(((uint16_t)~0u) << (16u - n)) : 0u;
return (int16_t)((u >> n) | m);
}
static inline int32_t shra32(int32_t v, unsigned n) {
if (!n) return v;
uint32_t u = (uint32_t)v, sign = (u >> 31) & 1u;
uint32_t m = sign ? (~(uint32_t)0u << (32u - n)) : 0u;
return (int32_t)((u >> n) | m);
}
/* ══════════════════════════════════════════════════════════════════════
* 1D INTERPOLATION SLOT
* Layout matches family-1's pwm_interp_slot_t and the family-1 ROM
* convention [count*2, range, offset, seg_bytes] — t06211's FUN_6fb8
* fingerprints identically, so the same layout applies.
* ══════════════════════════════════════════════════════════════════════ */
typedef struct pwm_interp_slot {
int16_t row_stride; /* count * 2 (byte stride for Y-table rows) */
int16_t x_interval; /* x[k-1] - x[k] */
int16_t x_offset; /* input - x[k] */
int16_t y_byte_off; /* k * 2 */
} pwm_interp_slot_t;
/* ══════════════════════════════════════════════════════════════════════
* EXTERNAL INPUTS
* Populated each cycle by pwm_service via the getter vtable.
* t06211-specific set: no temperature, no angle_dec_cmd in the setpoint
* (the family-2 setpoint is single-submap RPM-indexed).
* ══════════════════════════════════════════════════════════════════════ */
typedef struct pwm_inputs {
int16_t ckp_in; /* 0x02f8 — sensed position */
uint16_t rpm; /* 0x0040 — engine RPM */
int16_t angle_dec_cmd; /* 0x0042 — accepted for family-1 API parity; unused here */
int16_t inj_qty_demand; /* 0x0044 — CAN: inj quantity demand (PI large-neg gate) */
int16_t b_fb_kw; /* CAN: plunger feedback baseline. Gets copied into
can_raw_b_fb_kw and decoded by s_setpoint_can_decode
(port of FUN_64c3) to produce target. */
int16_t state_130; /* 0x0130 — CAN: open/closed-loop discriminant */
uint16_t supply_voltage; /* 0x0142 — shape_eval submap input */
int16_t temperature; /* 0x0146 — accepted for family-1 API parity; unused here */
} pwm_inputs_t;
/** Getter vtable. 8-callback layout matches family-1's pwm_input_getters_t
* so a single FBKW.c can drive either family's pwm.c without changes.
* t06211 does not consume angle_dec_cmd or temperature; those getters are
* called and stored into rt->inputs but the pipeline ignores them. */
typedef struct pwm_input_getters {
int16_t (*ckp_in) (void *ctx);
uint16_t (*rpm) (void *ctx);
int16_t (*angle_dec_cmd) (void *ctx); /* accepted; unused by t06211 */
int16_t (*inj_qty_demand)(void *ctx);
int16_t (*b_fb_kw) (void *ctx);
int16_t (*state_130) (void *ctx);
uint16_t (*supply_voltage)(void *ctx);
int16_t (*temperature) (void *ctx); /* accepted; unused by t06211 */
void *ctx;
} pwm_input_getters_t;
typedef struct pwm_calibration pwm_calibration_t;
typedef struct pwm_submap_descr pwm_submap_descr_t;
/* ══════════════════════════════════════════════════════════════════════
* RUNTIME STATE
* RAM-address comments reference the t06211 MCS-96 map (see
* families/t06211/docs/variable-glossary.md).
* ══════════════════════════════════════════════════════════════════════ */
typedef struct pwm_runtime {
/* External inputs — refreshed each cycle from getters */
pwm_inputs_t inputs;
/* Async flags.
* reset_flag — set on reset edge; supervisor clears.
* system_flags_110 — bit 5 forces open-loop; bit 0 is fast-recovery latch.
*/
uint8_t reset_flag; /* 0x002e */
uint8_t system_flags_110; /* 0x0110 */
/* ── Dual-target setpoint architecture ── */
/* target (RW5E = RAM[0x005e]): PRIMARY CAN-driven setpoint
* computed by FUN_64c3 (called from CAN parser FUN_6192). Drives
* the PI controller in both open-loop (FUN_67c4:67e5) and
* closed-loop (FUN_67c4:680a) paths, and supervisor error compute
* (FUN_7beb:7c20). Formula: raw/2 + setpoint_offset_150 + rw42_state,
* clamped >= cal->target_5e_min_clamp. */
int16_t target_5e; /* 0x005e — primary CAN-decoded setpoint */
/* target_336 (DAT_0336): SECONDARY RPM-derived ceiling computed by
* FUN_7168(setpoint_descr) + FUN_77b3:77c7. Used ONLY as upper-clamp
* in FUN_672b PI compensation (0x67a9/0x67b0) and FUN_7beb supervisor
* (0x7c30/0x7c37). Does not drive control directly. */
int16_t target_336; /* 0x0336 — RPM-derived ceiling */
/* CAN-staging buffers — written externally before pwm_service so
* s_setpoint_can_decode can transform them into target. */
int16_t can_raw_b_fb_kw; /* 0x012c — raw b_fb_kw from CAN */
int16_t can_aux_12e; /* 0x012e — secondary CAN field (drives rw42_state) */
int16_t can_half_12a; /* 0x012a — cached raw>>1 (debug visibility) */
/* setpoint_offset_150 (RAM[0x0150]) is copied at runtime_init from
* cal.setpoint_offset (mirrors family-1's pattern at
* compact_src/pwm.c:66). The REAL ROM derives it at boot via
* FUN_6b7b @ 0x6bb5: RAM[0x150] = cal+0x4c - RW1E. Because RW1E's
* source at the call site (0x6c46) is in a Ghidra-unrecognised code
* region, we expose the field directly through cal so the user can
* set it to the match-fitted value without needing to simulate the
* full FUN_6b7b boot path. */
int16_t setpoint_offset_150; /* 0x0150 */
int16_t rw42_state; /* RW42 register — = can_aux_12e when valid */
int16_t b_fb_kw_baseline; /* 0x033a — latched snapshot of b_fb_kw on reset */
int16_t compensation_angle; /* 0x02b8 — PI-helper output consumed in supervisor */
int16_t angle_error_raw; /* 0x033e — supervisor output (raw + ceiling-clamped) */
uint16_t cl_enable_counter; /* 0x033c */
/* ── CL correction (RAM-layout identical to family 1) ── */
int16_t cl_correction_raw; /* 0x0176 */
int16_t angle_offset; /* 0x017c */
int16_t supervisor_state_17e; /* 0x017e — CL accumulator */
int16_t pos_error_normalizer; /* 0x0332 */
int16_t neg_error_normalizer; /* 0x0334 */
/* ── Publish + PI ── */
int16_t estimated_angle; /* 0x02cc */
int16_t angle_error_pi; /* 0x02be */
int16_t active_request; /* 0x0046 (RW46) — PI output / PWM feed-forward */
uint8_t pi_open_loop_flag; /* 0x02c4 */
uint8_t pi_shape_flag; /* 0x02c5 */
uint8_t pi_flag_c6; /* 0x02c6 */
uint8_t pi_flag_338; /* 0x0338 */
/* PI Block-4 error-window bounds (independent int16s, NOT a 32-bit
* integrator). Boot-initialised once by FUN_76aa @ 0x76aa — see
* docs/open-questions.md §2 (resolved). Trim bytes at 0x0414/0x0416
* have no writers in the ROM, so bounds default to ±853 at runtime. */
int16_t pi_error_bound_pos; /* 0x0450 — default +853 */
int16_t pi_error_bound_neg; /* 0x0452 — default -853 */
int16_t pi_state_118; /* 0x0118 */
int16_t pi_state_c2; /* 0x02c2 */
/* PI integrator at {RAM[0x02b4]:RAM[0x02b6]} — was misnamed "b_fb_kw"
* across the original glossary. Disasm `disasm_nav rw 0x02b4` shows
* only internal writers (FUN_672b, FUN_76aa, FUN_7c85), confirming
* this is purely a PI integrator state, not an external/CAN input.
* Treated as a signed 32-bit pair: hi at 0x02b4 (the value used in
* active_request = comp + pi_b4_state), lo at 0x02b6. */
int16_t pi_b4_state; /* 0x02b4 — integrator high word */
int16_t pi_b6_state; /* 0x02b6 — integrator low word */
/* PI compensation scalars — boot-set by FUN_76aa from cal+0x118/0x11A
* with optional <<4 byte trims at RAM[0x0410]/[0x0412] (no writers,
* default 0). Defaults: 0x0454=+480 (post-scale), 0x0456=+256 (step). */
int16_t pi_post_scale_454; /* 0x0454 — multiplier in comp = (err*scale)>>8 */
int16_t pi_integ_step_456; /* 0x0456 — multiplier in FUN_7c85 integrator step */
/* PI integ gain — boot-set by FUN_76aa from cal+0x11C (byte, clamped
* ≤15). Used in the "reset" branch as: pi_b4_state = (err*gain)>>4 + ckp.
* The runtime field formerly named "pwm_period" was a holdover from
* family 1 where 0x0330 had a different semantic; in t06211, 0x0330
* is this PI gain. */
uint8_t pi_integ_gain_330; /* 0x0330 — boot default 6 (cal+0x11C) */
/* Anti-windup flag set by FUN_672b clamps; consulted by FUN_7c85
* to gate integration direction. Values: 0 (in-range), 1 (clamped
* low), 2 (clamped high). */
uint8_t pi_flag_c7; /* 0x02c7 */
/* ── PWM output ── */
uint16_t pwm_duty; /* 0x02d2 */
uint16_t pwm_on_time; /* 0x02ce */
uint16_t pwm_off_time; /* 0x02d0 */
/* pwm_period — total period in Timer2 ticks = on_time + off_time.
* Mirrored from t06211 RAM[0x02e4] (computed each cycle by the PWM
* output stage). Exposed for family-1 FBKW.c API parity
* (UpdatePWM(&htim4, ch, pwm_on_time, pwm_period)). */
uint16_t pwm_period;
uint8_t pwm_duty_range_flag; /* 0x00d1 */
pwm_interp_slot_t pwm_slot_a; /* 0x02d4 */
pwm_interp_slot_t pwm_slot_b; /* 0x02dc */
/* pwm_shape_state[6] mirrors RAM[0x02e4..0x02ee]:
* [0] pwm_period working value (RAM 0x02e4)
* [1] (unused — was slew_increment, broken out as field below)
* [2] (RAM 0x02e8 — ROM band-array ptr, unused in C)
* [3] (RAM 0x02ea — ROM halfwidth ptr, unused in C)
* [4] (RAM 0x02ec — slew step magnitude, unused; read from cal)
* [5] shape_height (RAM 0x02ee, E2 output) */
int16_t pwm_shape_state[6]; /* 0x02e4-0x02ee */
/* Persistent slew_increment (RAM[0x02e6]). Carried across cycles so
* the hysteresis-margin HOLD path can preserve previous direction.
* Subtracted from pwm_period each cycle. See open-questions §5. */
int16_t pwm_slew_increment; /* 0x02e6 */
/* ── Bindings (set once by pwm_init) ── */
const pwm_calibration_t *bound_cal;
const pwm_input_getters_t *bound_getters;
} pwm_runtime_t;
/* ── Calibration (decoded ROM values) ───────────────────────────────── */
struct pwm_calibration {
/* Scalars (from families/t06211/cal_offsets.py FLASH_OFFSETS) */
int16_t large_pos_error_thresh; /* cal+0x10E */
int16_t large_neg_error_thresh; /* cal+0x110 */
int16_t pi_low_clamp; /* cal+0x120 = -512 — Block-4 low clamp + open-loop lower bound */
int16_t pi_high_clamp; /* cal+0x124 = +1707 — Block-4 high clamp */
/* CAN-decoded setpoint (FUN_64c3) cal constants */
int16_t b_fb_kw_upper_bound; /* cal+0x004 = +7680 — raw b_fb_kw upper sanity bound */
int16_t b_fb_kw_lower_bound; /* cal+0x006 = -768 — raw b_fb_kw lower sanity bound */
/* setpoint_offset — static bias added after halving raw b_fb_kw.
* Family-1 analog: CAL+0x0052 - CAL+0x0054 (compact_src/pwm.h:210).
* For t06211, FUN_6b7b computes the equivalent as cal+0x4c - cal+0x4e
* (= 3499 - 4156 = -657) at boot and stores at RAM[0x150].
* Copied into runtime.setpoint_offset_150 by runtime_reset. */
int16_t setpoint_offset; /* = cal+0x4c - cal+0x4e */
int16_t target_5e_min_clamp; /* cal+0x122 = -512 — RW5E lower clamp */
int16_t can_aux_12e_max; /* cal+0x002 — upper bound for RAM[0x12e] (FUN_649e) */
int16_t error_thresh_114; /* cal+0x114 */
int16_t pi_thresh_116; /* cal+0x116 */
/* pi_state_118 saturation threshold — when the recovery counter
* reaches this, FUN_66a8 latches bit0 of system_flags_110, zeroes
* the counter, and reloads pi_state_c2 from error_thresh_114.
* Boot-cached at RAM[0x02c0] by FUN_6b7b:0x6bd4. ROM literal 0x0320 = 800. */
int16_t pi_sat_count_threshold; /* cal+0x112 */
int16_t rpm_threshold_11E; /* cal+0x11E — RPM gate inside FUN_66a8 */
/* Closed-loop entry RPM floor (FUN_67c4:0x67d9). Sourced from
* RAM[0x605c] (flash mirror; ROM literal 0x01A4 = 420). */
int16_t pi_cl_rpm_floor;
int16_t pwm_detail_x0; /* cal+0x0EE */
int16_t pwm_detail_x1; /* cal+0x0F0 */
int16_t pwm_cached_ptr_0F2; /* cal+0x0F2 — first RPM-window breakpoint (legacy scalar) */
int16_t pwm_cached_ptr_102; /* cal+0x102 — window halfwidth (legacy scalar) */
int16_t pwm_const_104; /* cal+0x104 */
/* RPM-window matching for pwm_period slew (ROM 0x538b-0x5575).
* Eight breakpoints at cal+0xF2 define four bands (lo,hi); the
* cal+0x102 halfwidth adds hysteresis. RPM INSIDE any band drives
* pwm_period toward pwm_period_min (→ non-zero shape contribution
* adds ~49 duty ticks). RPM OUTSIDE all bands drives pwm_period
* toward pwm_period_max (→ zero contribution; Y-table floor). */
int16_t pwm_rpm_windows[8]; /* cal+0xF2 — 4 (lo,hi) pairs */
int16_t pwm_window_halfwidth; /* cal+0x102 — hysteresis halfwidth */
/* Per-cycle slew step magnitude for pwm_period. Sourced from
* cal+0x104 (= 354); cached to RAM[0x02ec] at FUN_5314:0x53b9 and
* read at the Phase 1 and Phase 3 sites. Aliases pwm_const_104 —
* same offset, semantic name. */
int16_t pwm_slew_step; /* cal+0x104 — slew magnitude */
const int16_t *pwm_y_table; /* cal+0x154 (indirect) */
const int16_t *shape_y_table; /* cal+0x15E (indirect) */
int16_t closed_loop_gain_const; /* cached at ROM 0x6056 = 0x000A */
uint16_t pwm_period_min; /* hypothesised; see cal_tables_rom.c */
uint16_t pwm_period_max; /* hypothesised */
/* PWM duty clamp bounds — RAM[0x6058]/RAM[0x605a] flash mirrors.
* Defaults 0x00CD/0x0F32 match the default family's pwm_min/pwm_max. */
uint16_t pwm_min; /* RAM[0x6058] cache */
uint16_t pwm_max; /* RAM[0x605a] cache */
};
/* ── Submap descriptor ──────────────────────────────────────────────── */
struct pwm_submap_descr {
uint16_t flags; /* +0 */
const int16_t *input_ptr; /* +2 (bound at runtime) */
uint16_t count; /* +4 */
const int16_t *x; /* +6 */
uint16_t input_addr;
};
/** Bind descriptor input_ptr fields to rt inputs. */
static inline void pwm_bind_submap_inputs(
pwm_runtime_t *rt,
pwm_submap_descr_t *descrs,
uint16_t n)
{
for (uint16_t i = 0; i < n; i++) {
switch (descrs[i].input_addr) {
case 0x0040: descrs[i].input_ptr = (const int16_t *)&rt->inputs.rpm; break;
case 0x0044: descrs[i].input_ptr = &rt->inputs.inj_qty_demand; break;
case 0x0046: descrs[i].input_ptr = &rt->active_request; break;
case 0x0142: descrs[i].input_ptr = (const int16_t *)&rt->inputs.supply_voltage; break;
default: descrs[i].input_ptr = NULL; break;
}
}
}
/* ══════════════════════════════════════════════════════════════════════
* PUBLIC API
* ══════════════════════════════════════════════════════════════════════ */
/** pwm_flash_t — placeholder for family-1 API parity. Family-1 has Y-tables
* in a separate `pwm_flash_t` struct; t06211 keeps them inside
* pwm_calibration_t so this struct is empty. Kept so FBKW.c / callers
* can pass &pwm_flash_rom without the call failing. */
typedef struct pwm_flash { char _unused; } pwm_flash_t;
/** 4-argument init matching family-1's pwm_init signature. The `flash`
* pointer is accepted (must be non-NULL — pass &pwm_flash_rom) and
* ignored by the t06211 pipeline. */
void pwm_init(pwm_runtime_t *rt,
const pwm_calibration_t *cal,
const pwm_flash_t *flash,
const pwm_input_getters_t *getters);
void pwm_service(pwm_runtime_t *rt);
/** @brief CKP-edge reset hook — call from the CKP interrupt handler.
*
* Sets rt->reset_flag = 1 so the next pwm_service() call zeroes the
* closed-loop accumulator (supervisor_state_17e) and the enable counter
* (cl_enable_counter). Byte store is atomic on all mainstream targets;
* no locking required as long as it is not called concurrently with
* pwm_service().
*
* Pattern parity with the default family. See
* docs/re-guide-ckp-reset-pattern.md for the full rationale.
*
* **Variant note for t06211:** the reset_flag producer is **absent from
* the ROM** (no STB #1 site writing to 0x002e). The supervisor at 0x7beb
* still consumes reset_flag == 1 and clears it, but nothing inside the
* ROM ever sets it. Consequence: the accumulator walk-off hazard is
* structurally identical to family-1, and the application layer must
* drive this hook itself at engine-rev rate (typically 4 pulses per
* revolution on VP44, ~1215 scheduler cycles apart at 1200 rpm with a
* 1 kHz scheduler). Without it, supervisor_state_17e integrates forever.
*/
static inline void pwm_ckp_isr(pwm_runtime_t *rt) { rt->reset_flag = 1; }
/** Utility: 1D descending-X piecewise-linear lookup. */
int16_t pwm_interp_lookup(const int16_t *x, const int16_t *y,
uint16_t n, int16_t in);
/** Utility: bypass-PI bilinear LUT — eval(pwm_A, rpm), eval(pwm_B, fbkw),
* combine over Y-table, then apply [205, 3890] clamp. Use this to query
* the ROM Y-table directly as a (rpm, fbkw) lookup, skipping the PI
* controller entirely. Returns the clamped duty. */
uint16_t pwm_lut_duty(const pwm_calibration_t *cal,
uint16_t rpm, int16_t fbkw);
/* ── ROM-decoded cal + flash placeholder (defined in cal_tables_rom.c) ── */
extern const pwm_calibration_t pwm_cal_rom;
extern const pwm_flash_t pwm_flash_rom;
/** Descriptor array indices. */
enum pwm_submap_id {
PWM_SUBMAP_SETPOINT_INTERP = 0,
PWM_SUBMAP_PWM_A = 1,
PWM_SUBMAP_PWM_B = 2,
PWM_SUBMAP_SHAPE_EVAL = 3,
PWM_SUBMAP_COUNT = 4
};
extern pwm_submap_descr_t pwm_submap_descrs[PWM_SUBMAP_COUNT];
extern const int16_t *pwm_submap_y_of(uint16_t idx);
#endif /* PWM_H */