/** * @file pwm.c (families/t06211/compact_src) * @brief Compact single-file implementation of the t06211 PWM control * pipeline. Merges the 10 per-module files from * families/t06211/src/ into one translation unit. * * Pipeline (t06211 FUN_77b3 @ 0x77b3): * 1. setpoint (FUN_7168) — single-submap RPM-indexed interp * 2. supervisor (FUN_7beb) — reset + counter + error + clamp * 3. publish_cl (FUN_7cd8) — calls cl_correction, publishes est_angle * 4. pi_controller (FUN_67c4) — open/closed-loop branch * 5. pwm_output (FUN_5314) — eval+eval+combine+saturate+HW shadow * * Per-block address citations follow the families/t06211/src/ per-module * ports verbatim; see those files for expanded commentary. */ #include "pwm.h" /* Forward decls for file-static helpers. */ static void s_eval_submap(const pwm_submap_descr_t *d, pwm_interp_slot_t *slot); static int16_t s_combine(const int16_t *y_base, const pwm_interp_slot_t *sa, const pwm_interp_slot_t *sb); static int16_t s_refine(const int16_t *y_base, const pwm_interp_slot_t *slot); static void s_setpoint (pwm_runtime_t *rt); static void s_supervisor (pwm_runtime_t *rt); static void s_cl_correct (pwm_runtime_t *rt, const pwm_calibration_t *cal); static void s_publish_cl (pwm_runtime_t *rt, const pwm_calibration_t *cal); static void s_pi_update (pwm_runtime_t *rt, const pwm_calibration_t *cal); static void s_pwm_output (pwm_runtime_t *rt, const pwm_calibration_t *cal); /* ═════════════════════════════════════════════════════════════════════ * Init / binding * ═════════════════════════════════════════════════════════════════════ */ static void runtime_reset(pwm_runtime_t *rt) { memset(rt, 0, sizeof(*rt)); /* Normalizers are ROM-resident; fallback defaults for safety. */ rt->pos_error_normalizer = 500; rt->neg_error_normalizer = 650; /* PI Block-4 error-window bounds — mirror the one-shot boot init * done by FUN_76aa in the real ROM. Scratch bytes at RAM[0x0414] / * [0x0416] have no writers anywhere in the ROM (EEPROM trim bytes, * default 0), so the bounds resolve to cal+0x10A (=+853) and * cal+0x10C (=-853). See docs/open-questions.md §2. */ rt->pi_error_bound_pos = +853; rt->pi_error_bound_neg = -853; /* PI compensation/integrator scalars — defaults from FUN_76aa * boot init. cal+0x118=+480, cal+0x11A=+256, cal+0x11C=6 (byte). */ rt->pi_post_scale_454 = 480; rt->pi_integ_step_456 = 256; rt->pi_integ_gain_330 = 6; /* CAN-setpoint defaults. setpoint_offset_150 mirrors family-1's * pwm_init copy pattern (compact_src/pwm.c:66) — it's a cal-derived * static bias, not a runtime value. */ rt->rw42_state = 0; rt->can_raw_b_fb_kw = 0; rt->can_aux_12e = 0; rt->target_5e = 0; rt->target_336 = 0; } static void apply_cal(pwm_runtime_t *rt, const pwm_calibration_t *cal) { /* Mirrors family-1's pwm_init copy of cal→runtime statics. */ rt->setpoint_offset_150 = cal->setpoint_offset; } void pwm_init(pwm_runtime_t *rt, const pwm_calibration_t *cal, const pwm_flash_t *flash, const pwm_input_getters_t *getters) { (void)flash; /* family-1 API parity; t06211 keeps Y-tables in cal */ runtime_reset(rt); rt->bound_cal = cal; rt->bound_getters = getters; apply_cal(rt, cal); pwm_bind_submap_inputs(rt, pwm_submap_descrs, PWM_SUBMAP_COUNT); } static void read_inputs(pwm_runtime_t *rt) { const pwm_input_getters_t *g = rt->bound_getters; void *ctx = g->ctx; rt->inputs.ckp_in = g->ckp_in (ctx); rt->inputs.rpm = g->rpm (ctx); rt->inputs.angle_dec_cmd = g->angle_dec_cmd (ctx); /* accepted; unused */ rt->inputs.inj_qty_demand = g->inj_qty_demand(ctx); rt->inputs.b_fb_kw = g->b_fb_kw (ctx); rt->inputs.state_130 = g->state_130 (ctx); rt->inputs.supply_voltage = g->supply_voltage(ctx); rt->inputs.temperature = g->temperature (ctx); /* accepted; unused */ /* The b_fb_kw getter result drives the CAN-decoded setpoint chain * (FUN_64c3). Mirrors the real ROM where RAM[0x12c] is written by * the CAN parser before FUN_64c3 runs. */ rt->can_raw_b_fb_kw = rt->inputs.b_fb_kw; } /* ═════════════════════════════════════════════════════════════════════ * Interpolation — FUN_7168 (0x7168-0x71d7), fingerprint #3 * Raw helper; also drives the setpoint stage via descriptor. * ═════════════════════════════════════════════════════════════════════ */ int16_t pwm_interp_lookup(const int16_t *x, const int16_t *y, uint16_t n, int16_t in) { if (in >= x[0]) return y[0]; if (in <= x[n - 1u]) return y[n - 1u]; uint16_t k = 1u; while (k < n && in < x[k]) { k++; } int16_t x_k = x[k]; int16_t num = (int16_t)(in - x_k); int16_t dx = (int16_t)(x_k - x[k - 1u]); int16_t dy = (int16_t)(y[k] - y[k - 1u]); int32_t prod = MUL_S16(num, dy); int16_t quot = (int16_t)(prod / (int32_t)dx); /* Disasm 0x71c5: ADD RW1C, -0x2[RW20]. After the disasm's pointer * advance to the y[] half (0x71b2 ADD RW20, RW1E), RW20 points one * past the breakpoint where the search settled, so -0x2[RW20] is * y at that breakpoint — which is y[k] in C's k-naming (k = first * index where in >= x[k] given descending x). Earlier this returned * y[k-1], producing wrong-polarity setpoints (e.g. target_336 = 1962 * instead of 1195 at rpm=3355). */ return (int16_t)(quot + y[k]); } /* ═════════════════════════════════════════════════════════════════════ * Submap eval / bilinear combine / refine — t06211 FUN_6fb8 / FUN_7035 / * FUN_7014. Scratch layout matches the family-1 ROM convention. * ═════════════════════════════════════════════════════════════════════ */ static void s_eval_submap(const pwm_submap_descr_t *d, pwm_interp_slot_t *slot) { int16_t input = d->input_ptr ? *d->input_ptr : 0; slot->row_stride = (int16_t)(d->count * 2u); if (d->count == 0 || d->x == NULL) { slot->x_interval = 2; slot->x_offset = 2; slot->y_byte_off = 2; return; } uint16_t k; for (k = 1u; k < d->count; k++) { if (input >= d->x[k]) break; } if (k == 1u && input >= d->x[0]) { /* Upper-clamp sentinel [_, 2, 2, 2] */ slot->x_interval = 2; slot->x_offset = 2; slot->y_byte_off = 2; return; } if (k >= d->count) { k = (uint16_t)(d->count - 1u); } slot->x_interval = (int16_t)(d->x[k - 1u] - d->x[k]); slot->x_offset = (int16_t)(input - d->x[k]); slot->y_byte_off = (int16_t)(k * 2u); } static int16_t s_combine(const int16_t *y_base, const pwm_interp_slot_t *sa, const pwm_interp_slot_t *sb) { if (!y_base || !sa || !sb) return 0; int16_t den_a = sa->x_interval ? sa->x_interval : 1; int16_t den_b = sb->x_interval ? sb->x_interval : 1; int32_t row_off = MUL_S16(sb->y_byte_off, sa->row_stride) / 2; const uint8_t *yp_b = (const uint8_t *)y_base + row_off + sa->y_byte_off; const int16_t *yp = (const int16_t *)yp_b; int16_t y_here = *yp; int16_t y_prev_a = *(const int16_t *)(yp_b - 2); int32_t diff_a = MUL_S16((int16_t)(y_prev_a - y_here), sa->x_offset); int16_t rowB = (int16_t)(y_here + (int32_t)(diff_a / (int32_t)den_a)); const uint8_t *yp_b_prev = yp_b - sa->row_stride; const int16_t *yp_prev = (const int16_t *)yp_b_prev; int16_t y_here_pb = *yp_prev; int16_t y_prev_a_pb = *(const int16_t *)(yp_b_prev - 2); int32_t diff_a_pb = MUL_S16((int16_t)(y_prev_a_pb - y_here_pb), sa->x_offset); int16_t rowBp = (int16_t)(y_here_pb + (int32_t)(diff_a_pb / (int32_t)den_a)); int32_t diff_b = MUL_S16((int16_t)(rowBp - rowB), sb->x_offset); return (int16_t)(rowB + (int32_t)(diff_b / (int32_t)den_b)); } static int16_t s_refine(const int16_t *y_base, const pwm_interp_slot_t *slot) { if (!y_base || !slot) return 0; int16_t den = slot->x_interval ? slot->x_interval : 1; const uint8_t *yp_b = (const uint8_t *)y_base + slot->y_byte_off; const int16_t *yp = (const int16_t *)yp_b; int16_t y_here = *yp; int16_t y_prev_a = *(const int16_t *)(yp_b - 2); int32_t diff = MUL_S16((int16_t)(y_prev_a - y_here), slot->x_offset); return (int16_t)(y_here + (int32_t)(diff / (int32_t)den)); } /* Descriptor-driven wrapper used only by the setpoint stage. */ static int16_t s_interp_descr(const pwm_submap_descr_t *d, const int16_t *y_array) { int16_t input = d->input_ptr ? *d->input_ptr : 0; return pwm_interp_lookup(d->x, y_array, d->count, input); } /* ═════════════════════════════════════════════════════════════════════ * Stage 1 — Setpoint (FUN_77b3:77b3-77c7 + FUN_7168) * ═════════════════════════════════════════════════════════════════════ */ static void s_setpoint(pwm_runtime_t *rt) { const pwm_submap_descr_t *d = &pwm_submap_descrs[PWM_SUBMAP_SETPOINT_INTERP]; const int16_t *y = pwm_submap_y_of(PWM_SUBMAP_SETPOINT_INTERP); rt->target_336 = s_interp_descr(d, y); } /* ═════════════════════════════════════════════════════════════════════ * Stage 1b — CAN-decoded setpoint (FUN_64c3 @ 0x64c3-0x650a) * Real ROM call site: FUN_6192:0x61cc (CAN parser dispatcher). * For the C model, invoked from pwm_service after read_inputs so the * harness's per-cycle b_fb_kw value flows into target before PI runs. * Skip the RE7 gate and error path (sim trusts caller). * ═════════════════════════════════════════════════════════════════════ */ static void s_setpoint_can_decode(pwm_runtime_t *rt, const pwm_calibration_t *cal) { int16_t raw = rt->can_raw_b_fb_kw; /* [0x64ca-0x64d6] Bounds check */ if (raw > cal->b_fb_kw_upper_bound) return; if (raw < cal->b_fb_kw_lower_bound) return; /* [0x64dd] half = raw >> 1 (SHRA — sign-extending) */ int16_t half = shra16(raw, 1); rt->can_half_12a = half; /* [0x64e5-0x64ea] result = half + RAM[0x150] + RW42 */ int16_t result = (int16_t)(half + rt->setpoint_offset_150 + rt->rw42_state); /* [0x64ed-0x64f9] Lower clamp */ if (result < cal->target_5e_min_clamp) { result = cal->target_5e_min_clamp; } rt->target_5e = result; } /* ═════════════════════════════════════════════════════════════════════ * Stage 2 — Supervisor (FUN_7beb @ 0x7beb-0x7c41) * ═════════════════════════════════════════════════════════════════════ */ static void s_supervisor(pwm_runtime_t *rt) { /* Reset-flag branch (0x7beb-0x7c0c) */ if (rt->reset_flag == 1u) { rt->reset_flag = 0u; rt->cl_enable_counter = 0u; rt->supervisor_state_17e = 0; rt->b_fb_kw_baseline = rt->inputs.b_fb_kw; } else { /* Counter tick (0x7c11-0x7c1b) */ rt->cl_enable_counter = (uint16_t)(rt->cl_enable_counter + 1u); } /* Error compute (0x7c20-0x7c2b) * Disasm: SUB RW1C, RW5E, DAT_02f8 — primary setpoint is target. */ int16_t error = (int16_t)(rt->target_5e - rt->inputs.ckp_in); error = (int16_t)(error + rt->compensation_angle); rt->angle_error_raw = error; /* Ceiling clamp (0x7c30-0x7c41) * Disasm: CMP RW1C, DAT_0336 — clamp uses RPM-derived ceiling. */ if (error > rt->target_336) { rt->angle_error_raw = rt->target_336; } } /* ═════════════════════════════════════════════════════════════════════ * Stage 3a — Closed-loop correction (FUN_5f1f @ 0x5f1f-0x5f66) * Fingerprint #10 bit-for-bit match to family-1 algorithm. * ═════════════════════════════════════════════════════════════════════ */ static void s_cl_correct(pwm_runtime_t *rt, const pwm_calibration_t *cal) { int16_t correction; /* Gate on low byte of cl_enable_counter (0x5f1f-0x5f24). */ if ((rt->cl_enable_counter & 0xFFu) == 0u) { correction = 0; } else { int16_t normalizer = (rt->angle_error_raw > 0) ? rt->pos_error_normalizer : rt->neg_error_normalizer; int32_t product = MUL_S16(rt->angle_error_raw, cal->closed_loop_gain_const); product = (int32_t)((uint32_t)product << 4); correction = (normalizer != 0) ? (int16_t)(product / (int32_t)normalizer) : (int16_t)0; } rt->cl_correction_raw = correction; rt->supervisor_state_17e = (int16_t)(rt->supervisor_state_17e + correction); rt->angle_offset = shra16(rt->supervisor_state_17e, 4u); } /* ═════════════════════════════════════════════════════════════════════ * Stage 3 — Publish CL (FUN_7cd8 @ 0x7cd8-0x7cea) * ═════════════════════════════════════════════════════════════════════ */ static void s_publish_cl(pwm_runtime_t *rt, const pwm_calibration_t *cal) { s_cl_correct(rt, cal); rt->estimated_angle = (int16_t)(rt->inputs.ckp_in + rt->angle_offset); } /* ═════════════════════════════════════════════════════════════════════ * Stage 4 — PI controller (FUN_67c4 @ 0x67c4 + FUN_66a8 + FUN_672b) * ═════════════════════════════════════════════════════════════════════ */ /* FUN_66a8 — saturation-latch recovery handler. Role-equivalent to the * default family's `s_recovery` / `fast_recovery`; same name across * variants for cross-family alignment. Disasm/source mapping (audit * table — keep the cooldown gate and gated-increment branches in this * exact order): * 66ad-66b2 CMPB pi_shape_flag, pi_flag_c6 ; JNE LAB_671d * 66b9-66be CMP pi_state_118, [0x02c0]=cal+0x112 ; JNC LAB_66e6 * JNC ⇒ counter < threshold ⇒ gated-increment * fall-through ⇒ counter ≥ threshold ⇒ latch+reset * 66c0-66e5 latch bit0 ; counter = 0 ; pi_state_c2 ← cal+0x114 * 66e6-66f4 CMP rpm, [cal+0x11E] ; JLE exit * 66f6-6705 JBS bit4 / JBS bit5 — exit * 6706-670b CMP ZR, pi_state_c2 ; JNE exit * 670d-6717 pi_state_118 += 1 * 671d-6725 JBS bit0 exit ; pi_state_118 = 0 */ static void s_recovery(pwm_runtime_t *rt, const pwm_calibration_t *cal) { if (rt->pi_shape_flag != rt->pi_flag_c6) { if ((rt->system_flags_110 & 0x01u) == 0u) { rt->pi_state_118 = 0; } return; } /* Counter ≥ threshold → latch+reset. Threshold is boot-cached at * RAM[0x02c0] from cal+0x112 by FUN_6b7b:0x6bd4. */ if ((uint16_t)rt->pi_state_118 >= (uint16_t)cal->pi_sat_count_threshold) { rt->system_flags_110 = (uint8_t)(rt->system_flags_110 | 0x01u); rt->pi_state_118 = 0; rt->pi_state_c2 = cal->error_thresh_114; return; } /* Gated increment: rpm > rpm_threshold_11E required. */ if ((int16_t)rt->inputs.rpm <= cal->rpm_threshold_11E) return; /* Bits 4 or 5 of system_flags abort. */ if ((rt->system_flags_110 & 0x30u) != 0u) return; /* c2 cooldown gate — counter advances only after c2 has decayed to 0. */ if (rt->pi_state_c2 != 0) return; rt->pi_state_118 = (int16_t)(rt->pi_state_118 + 1); } /* FUN_7c85 @ 0x7c85-0x7cbf — PI integrator step. Anti-windup gates * the update direction against pi_flag_c7 (0=in-range, 1=clamped low, * 2=clamped high). Updates the {pi_b4_state:pi_b6_state} 32-bit pair. * Disasm: * 7c85 LD RW1C, err * 7c8a MUL RL1C, [0x0456] ; signed 32-bit, RL1C = err*step * 7c90 SHLL RL1C, #0x4 ; RL1C <<= 4 * 7c93 JGE LAB_7ca1 ; if signed result >= 0 * 7c95-7c9d if pi_flag_c7==1 RET ; (clamped-low → don't push more negative) * 7c9f SJMP LAB_7cab ; else update * LAB_7ca1: if pi_flag_c7==2 RET ; (clamped-high → don't push more positive) * LAB_7cab: integrator += RL1C ; ADD lo, ADDC hi */ static void s_pi_integrator_step(pwm_runtime_t *rt) { int32_t prod = MUL_S16(rt->angle_error_pi, rt->pi_integ_step_456); int32_t step = (int32_t)((uint32_t)prod << 4); if (step >= 0) { if (rt->pi_flag_c7 == 0x02) return; /* anti-windup: clamped-high */ } else { if (rt->pi_flag_c7 == 0x01) return; /* anti-windup: clamped-low */ } /* Combine {hi:lo} as int32 -> add step -> split back. */ int32_t accum = ((int32_t)(int16_t)rt->pi_b4_state << 16) | (uint16_t)rt->pi_b6_state; accum += step; rt->pi_b6_state = (int16_t)(accum & 0xFFFF); rt->pi_b4_state = (int16_t)((accum >> 16) & 0xFFFF); } /* FUN_672b @ 0x672b-0x67c3 — PI compensation + final clamp. * Two arms decided by (pi_flag_338==1 || pi_open_loop_flag==0): * "fresh" → reset pi_b4_state from a new (err*pi_gain>>4)+ckp formula * "settled" → step the integrator via FUN_7c85 * Then compute compensation_angle = (err * pi_post_scale_454) >> 8 * and active_request = comp + pi_b4_state, clamped to [pi_low_clamp, target]. */ static void s_pi_compensation(pwm_runtime_t *rt, const pwm_calibration_t *cal) { int16_t error = rt->angle_error_pi; /* [0x672b-0x673a] Branch select. */ bool fresh = (rt->pi_flag_338 == 0x01) || (rt->pi_open_loop_flag == 0u); if (fresh) { /* [LAB_673c 0x673c-0x6762] Reset pi_b4_state. Disasm: * STB ZRlo, DAT_0338 ; pi_flag_338 = 0 * LD RW1C, err * EXT RL1C ; sign-extend to 32-bit * MUL RL1C, [0x0330] ; signed: RL1C = err32 * pi_gain * SHRA RW1C, #0x4 ; LOW WORD shifted (high word discarded) * ADD RW1C, RW1C, ckp_in * ST RW1C, DAT_02b4 ; pi_b4_state = result */ rt->pi_flag_338 = 0; int32_t prod = MUL_S16(error, (int16_t)rt->pi_integ_gain_330); int16_t scaled = shra16((int16_t)(prod & 0xFFFF), 4); rt->pi_b4_state = (int16_t)(scaled + rt->inputs.ckp_in); } else { /* [LAB_6764 0x6764] LCALL FUN_7c85 — integrator step. */ s_pi_integrator_step(rt); } /* [LAB_6767 0x6767-0x6779] compensation_angle = (RL1C = post_scale*err) >> 8. * Disasm: * LD RW1C, [0x0454] ; post_scale (default +480) * MUL RL1C, err ; RL1C = post_scale * err (signed) * SHRAL RL1C, #0x8 ; arithmetic shift right 8 (32-bit) * ST RW1C, DAT_02b8 ; compensation_angle = lo16 */ int32_t comp32 = MUL_S16(rt->pi_post_scale_454, error); int16_t comp = (int16_t)(shra32(comp32, 8) & 0xFFFF); rt->compensation_angle = comp; /* [0x677a-0x677f] active_request = comp + pi_b4_state */ int16_t combined = (int16_t)(comp + rt->pi_b4_state); rt->active_request = combined; /* [0x6782-0x67a8] Lower clamp: if active_request < pi_low_clamp, * pin to pi_low_clamp and set pi_flag_c7=1. */ if (rt->active_request < cal->pi_low_clamp) { rt->active_request = cal->pi_low_clamp; rt->pi_flag_c7 = 0x01; return; } /* [LAB_67a9 0x67a9-0x67bd] Upper clamp: if active_request > target, * pin to target and set pi_flag_c7=2. */ /* Disasm 0x67a9-0x67b0: CMP RW46, DAT_0336 — upper clamp uses * the RPM-derived ceiling, not the CAN-decoded primary setpoint. */ if (rt->active_request > rt->target_336) { rt->active_request = rt->target_336; rt->pi_flag_c7 = 0x02; return; } /* [LAB_67be 0x67be-0x67c3] In-range: pi_flag_c7 = 0. */ rt->pi_flag_c7 = 0; } static void s_pi_update(pwm_runtime_t *rt, const pwm_calibration_t *cal) { /* PI CL-gate threshold = cached ROM[0x605c]. Disasm site: * FUN_67c4:67d9 `CMP RW1C, DAT_605c` (literal 0x01A4 = 420). In the * real ECU, state_130 is almost always negative (CAN inactive * sentinel) which forces OL at line 67d4 before this RPM gate is * reached. Lifted to cal->pi_cl_rpm_floor. */ int16_t rpm_floor = cal->pi_cl_rpm_floor; bool open_loop = false; if ((rt->system_flags_110 & 0x20u) != 0u) open_loop = true; else if (rt->inputs.state_130 < 0) open_loop = true; else if ((int16_t)rt->inputs.rpm < rpm_floor) open_loop = true; if (open_loop) { rt->pi_open_loop_flag = 0; /* Disasm 0x67e5: LD RW46, RW5E — open-loop assigns the CAN-decoded * primary setpoint, NOT the RPM-derived ceiling. */ rt->active_request = rt->target_5e; /* cal+0x120 is a LOWER bound here (disasm: JLT clamps when * active_request < limit). Same constant also serves as the * Block-4 low clamp, hence `pi_low_clamp`. See * families/t06211/src/pi_controller_t06211.c for the full-disasm * annotation. */ if (rt->active_request < cal->pi_low_clamp) { rt->active_request = cal->pi_low_clamp; } goto final_flag; } /* Disasm 0x680a: SUB RW1C, RW5E, DAT_02cc — CL error uses target. */ rt->angle_error_pi = (int16_t)(rt->target_5e - rt->estimated_angle); int16_t err = rt->angle_error_pi; if (err > cal->large_pos_error_thresh) { rt->pi_shape_flag = 0; s_recovery(rt, cal); } else if (err < cal->large_neg_error_thresh) { rt->pi_shape_flag = 0x01; if (rt->inputs.inj_qty_demand <= cal->pi_thresh_116) { rt->pi_state_118 = 0; } else { s_recovery(rt, cal); } } else { rt->pi_shape_flag = 0x20; if (rt->pi_state_c2 > 0) { rt->pi_state_c2 = (int16_t)(rt->pi_state_c2 - 1); } if (rt->pi_state_c2 == 0) { rt->system_flags_110 = (uint8_t)(rt->system_flags_110 & 0xFEu); } if (rt->pi_state_118 > 0) { rt->pi_state_118 = (int16_t)(rt->pi_state_118 - 1); } } /* ── Block 4: error-window decision [0x68b7-0x68ef] ── * Strict > / < per disasm JLE 68c9 / JGE 68ed. Bounds at * 0x0450/0x0452 are set once at boot by FUN_76aa — see * docs/open-questions.md §2 closeout. */ if (err > rt->pi_error_bound_pos) { rt->active_request = cal->pi_high_clamp; rt->pi_flag_338 = 0x01; } else if (err < rt->pi_error_bound_neg) { rt->active_request = cal->pi_low_clamp; rt->pi_flag_338 = 0x01; } else { s_pi_compensation(rt, cal); } final_flag: rt->pi_open_loop_flag = 0x01; rt->pi_flag_c6 = rt->pi_shape_flag; } /* ═════════════════════════════════════════════════════════════════════ * Stage 5 — PWM output (FUN_5314 @ 0x5314-0x565f) * ═════════════════════════════════════════════════════════════════════ */ static void s_pwm_output(pwm_runtime_t *rt, const pwm_calibration_t *cal) { /* A. eval pwm_A */ s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_PWM_A], &rt->pwm_slot_a); /* B. eval pwm_B */ s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_PWM_B], &rt->pwm_slot_b); /* C. combine bilinear */ int16_t duty = s_combine(cal->pwm_y_table, &rt->pwm_slot_a, &rt->pwm_slot_b); rt->pwm_duty = (uint16_t)duty; /* D. Duty-range classification [0x5361-0x538b] — based on pre-shape duty. */ if (rt->pwm_duty < 0x29u) rt->pwm_duty_range_flag = 1u; else if (rt->pwm_duty > 0xFD7u) rt->pwm_duty_range_flag = 2u; else rt->pwm_duty_range_flag = 0u; /* E1. RPM-window three-phase matcher [0x53be-0x552a]. * * Phase 1 (strict-band): rpm strictly inside any of 4 bands at * cal+0xF2 → slew_increment = +pwm_slew_step. * Phase 2 (hysteresis margin): rpm in any band's halfwidth-extended * region → HOLD (preserve previous slew_increment). * Phase 3 (deep-out): rpm clear of every extended band AND * pwm_period < pwm_period_max → slew_increment = -pwm_slew_step. * * Slew applied as `pwm_period -= slew_increment` (subtraction; * positive increment shrinks period → faster PWM). * pwm_slew_increment lives in rt and persists across cycles so the * HOLD path keeps the previous direction. * * Disasm note: high-edge tests use JC (db) at 0x5463/0x5497 for * bands 0/1 and JNC (d3) at 0x54cb/0x54fd for bands 2/3 — these * encodings produce equivalent control flow (HOLD on rpm < hi+hw, * advance on rpm >= hi+hw). See open-questions.md §5 closeout. */ uint16_t *pwm_period = (uint16_t *)&rt->pwm_shape_state[0]; const int16_t rpm_s = (int16_t)rt->inputs.rpm; const int16_t halfwidth = cal->pwm_window_halfwidth; const int16_t step_mag = cal->pwm_slew_step; /* Phase 1 — strict-band test [0x53be-0x542d] */ int slew_set = 0; for (int bi = 0; bi < 4; bi++) { int16_t lo = cal->pwm_rpm_windows[bi * 2]; int16_t hi = cal->pwm_rpm_windows[bi * 2 + 1]; if (rpm_s > lo && rpm_s < hi) { rt->pwm_slew_increment = step_mag; slew_set = 1; break; } } if (!slew_set) { /* Phase 2 — fine hysteresis margin [0x5434-0x54fe] */ int phase3 = 1; for (int bi = 0; bi < 4; bi++) { int16_t lo = cal->pwm_rpm_windows[bi * 2]; int16_t hi = cal->pwm_rpm_windows[bi * 2 + 1]; if (rpm_s <= (int16_t)(lo - halfwidth)) { continue; /* below extended low: try next band */ } if (rpm_s < (int16_t)(hi + halfwidth)) { phase3 = 0; /* HOLD */ break; } /* rpm_s >= hi + halfwidth: advance to next band */ } if (phase3 && *pwm_period < cal->pwm_period_max) { /* Phase 3 — deep-out negative slew [0x54ff-0x551b] */ rt->pwm_slew_increment = (int16_t)(-step_mag); } /* else: HOLD (no write) */ } /* Slew application [0x5520-0x552a] */ int32_t period_next = (int32_t)*pwm_period - (int32_t)rt->pwm_slew_increment; if (period_next < 0) period_next = 0; if (period_next > 0xFFFF) period_next = 0xFFFF; *pwm_period = (uint16_t)period_next; /* Final clamp [0x552f-0x5575] */ if (*pwm_period < cal->pwm_period_min) *pwm_period = cal->pwm_period_min; if (*pwm_period > cal->pwm_period_max) *pwm_period = cal->pwm_period_max; /* E2. Shape detail [0x557a-0x5608] */ pwm_interp_slot_t shape_slot; s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_SHAPE_EVAL], &shape_slot); int16_t shape_height = s_refine(cal->shape_y_table, &shape_slot); if (shape_height < 0) shape_height = 0; if (shape_height > 0x199) shape_height = 0x199; rt->pwm_shape_state[5] = shape_height; /* E3. Shape composition additive [0x55cb-0x560f]. ROM formula: * slope = (period_max - period_min) >> 8 * numerator = ((period_max - pwm_period) >> 8) * shape_height * pwm_duty += numerator / slope * Shifts happen before multiply (MCS-96 16-bit intermediate). */ int16_t slope = (int16_t)(((int32_t)cal->pwm_period_max - (int32_t)cal->pwm_period_min) >> 8); int16_t pmx_delta = (int16_t)(((int32_t)cal->pwm_period_max - (int32_t)*pwm_period) >> 8); int32_t shape_add = slope ? (MUL_S16(pmx_delta, shape_height) / (int32_t)slope) : 0; int32_t duty_new = (int32_t)rt->pwm_duty + shape_add; /* F. Duty bounds clamp [0x5614-0x5636] — ROM reads RAM[0x6058]/[0x605a] * (producers open-questions §3). Lifted to cal->pwm_min/cal->pwm_max * (defaults 205/3890, same as the default family). */ if (duty_new < (int32_t)cal->pwm_min) duty_new = (int32_t)cal->pwm_min; if (duty_new > (int32_t)cal->pwm_max) duty_new = (int32_t)cal->pwm_max; rt->pwm_duty = (uint16_t)duty_new; /* G. HW shadow writes [0x563b-0x565f] using the slewed pwm_period. */ uint32_t on_product = (uint32_t)(*pwm_period) * (uint32_t)rt->pwm_duty; rt->pwm_on_time = (uint16_t)(on_product / 0xFFFu); rt->pwm_off_time = (uint16_t)(*pwm_period - rt->pwm_on_time); rt->pwm_period = *pwm_period; } /* ═════════════════════════════════════════════════════════════════════ * Bypass-PI LUT helper: query the ROM Y-table directly with (rpm, fbkw) * as the two axes, applying the same eval+combine+clamp the PWM stage * does. Useful for plotting the static (rpm, fbkw) → duty surface * without the PI controller in the loop. * ═════════════════════════════════════════════════════════════════════ */ uint16_t pwm_lut_duty(const pwm_calibration_t *cal, uint16_t rpm, int16_t fbkw) { pwm_submap_descr_t a = pwm_submap_descrs[PWM_SUBMAP_PWM_A]; pwm_submap_descr_t b = pwm_submap_descrs[PWM_SUBMAP_PWM_B]; int16_t rpm_signed = (int16_t)rpm; a.input_ptr = &rpm_signed; b.input_ptr = &fbkw; pwm_interp_slot_t sa, sb; s_eval_submap(&a, &sa); s_eval_submap(&b, &sb); int16_t duty = s_combine(cal->pwm_y_table, &sa, &sb); if (duty < 205) duty = 205; if (duty > 3890) duty = 3890; return (uint16_t)duty; } /* ═════════════════════════════════════════════════════════════════════ * Public entry — pwm_service * Mirrors FUN_77b3 (0x77b3-0x77d8) — 5-call linear dispatcher. * ═════════════════════════════════════════════════════════════════════ */ void pwm_service(pwm_runtime_t *rt) { read_inputs(rt); const pwm_calibration_t *cal = rt->bound_cal; s_setpoint(rt); /* [0x77b3-0x77c7] target_336 = pwm_A interp(rpm) */ s_setpoint_can_decode(rt, cal); /* CAN-decoded target (FUN_64c3 in ROM) */ s_supervisor(rt); /* [0x77cc] */ s_publish_cl(rt, cal);/* [0x77cf] */ s_pi_update(rt, cal); /* [0x77d2] */ s_pwm_output(rt, cal);/* [0x77d5] */ }