740 lines
34 KiB
C
740 lines
34 KiB
C
/**
|
|
* @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 apply_cal(pwm_runtime_t *rt,
|
|
const pwm_calibration_t *cal)
|
|
{
|
|
/* All boot-derived constants come from cal — no literals here.
|
|
* Mirrors FUN_76aa @ 0x76aa, which copies cal+0x108/0x106 into
|
|
* RAM[0x0332]/[0x0334] (normalizers), cal+0x10A/0x10C into
|
|
* RAM[0x0450]/[0x0452] (P-shape bounds; trim bytes at RAM[0x414]/
|
|
* [0x416] have no writers in the ROM so the bases pass through),
|
|
* cal+0x118/0x11A into RAM[0x0454]/[0x0456] (P-gain + integ step;
|
|
* same trim-byte note for RAM[0x0410]/[0x0412]), and cal+0x11C
|
|
* (byte, clamped to 15) into RAM[0x0330]. */
|
|
rt->setpoint_offset = cal->setpoint_offset;
|
|
|
|
rt->p_shape_bound_pos = cal->init_p_shape_bound_pos;
|
|
rt->p_shape_bound_neg = cal->init_p_shape_bound_neg;
|
|
rt->p_gain_normal = cal->init_p_gain_normal;
|
|
rt->integ_step_normal = cal->init_integ_step_normal;
|
|
rt->open_loop_p_gain = cal->init_open_loop_p_gain;
|
|
|
|
rt->pos_error_normalizer = cal->init_pos_error_normalizer;
|
|
rt->neg_error_normalizer = cal->init_neg_error_normalizer;
|
|
}
|
|
|
|
static void runtime_reset(pwm_runtime_t *rt)
|
|
{
|
|
memset(rt, 0, sizeof(*rt));
|
|
}
|
|
|
|
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.cl_gate_input = g->cl_gate_input (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. pi_high_clamp_ceiling
|
|
* = 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->pi_high_clamp_ceiling = 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 + 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). Disasm:
|
|
* 7bf6 reset_flag = 0
|
|
* 7bfb cl_enable_counter (DAT_033c) = 0
|
|
* 7c00 supervisor_state (RW17E) = 0
|
|
* 7c05 LD RW1C, DAT_02b4 ; pi_integ_hi
|
|
* 7c0a ST RW1C, DAT_033a ; snapshot pi_integ_hi
|
|
* The DAT_033a store is a dead-store (no readers anywhere in the
|
|
* ROM); kept here for parity. The earlier port assigned
|
|
* rt->inputs.b_fb_kw to this slot — that was inherited from the
|
|
* family-1 idiom and is wrong for this variant. */
|
|
if (rt->reset_flag == 1u) {
|
|
rt->reset_flag = 0u;
|
|
rt->cl_enable_counter = 0u;
|
|
rt->supervisor_state = 0;
|
|
rt->pi_integ_hi_snapshot = rt->pi_integ_hi; /* dead-store parity */
|
|
} 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_5e. */
|
|
int16_t error = (int16_t)(rt->target_5e - rt->inputs.ckp_in);
|
|
error = (int16_t)(error + rt->pi_p_term);
|
|
rt->angle_error_raw = error;
|
|
|
|
/* Ceiling clamp (0x7c30-0x7c41)
|
|
* Disasm: CMP RW1C, DAT_0336 — clamp uses RPM-derived ceiling. */
|
|
if (error > rt->pi_high_clamp_ceiling) {
|
|
rt->angle_error_raw = rt->pi_high_clamp_ceiling;
|
|
}
|
|
}
|
|
|
|
/* ═════════════════════════════════════════════════════════════════════
|
|
* 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 = (int16_t)(rt->supervisor_state + correction);
|
|
rt->angle_offset = shra16(rt->supervisor_state, 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_integ_hi:pi_integ_lo} 32-bit pair.
|
|
* Disasm:
|
|
* 7c85 LD RW1C, err
|
|
* 7c8a MUL RL1C, [0x0456] ; signed 32-bit, RL1C = err*integ_step_normal
|
|
* 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->integ_step_normal);
|
|
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)rt->pi_integ_hi << 16)
|
|
| (uint16_t)rt->pi_integ_lo;
|
|
accum += step;
|
|
rt->pi_integ_lo = (int16_t)(accum & 0xFFFF);
|
|
rt->pi_integ_hi = (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_integ_hi from a new (err*open_loop_p_gain>>4)+ckp
|
|
* "settled" → step the integrator via FUN_7c85
|
|
* Then compute pi_p_term = (p_gain_normal * err) >> 8 and
|
|
* active_request = pi_p_term + pi_integ_hi, clamped to
|
|
* [pi_low_clamp, pi_high_clamp_ceiling].
|
|
*/
|
|
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_integ_hi. 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 * open_loop_p_gain
|
|
* SHRA RW1C, #0x4 ; LOW WORD shifted (high word discarded)
|
|
* ADD RW1C, RW1C, ckp_in
|
|
* ST RW1C, DAT_02b4 ; pi_integ_hi = result
|
|
*/
|
|
rt->pi_flag_338 = 0;
|
|
int32_t prod = MUL_S16(error, rt->open_loop_p_gain);
|
|
int16_t scaled = shra16((int16_t)(prod & 0xFFFF), 4);
|
|
rt->pi_integ_hi = (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] pi_p_term = (RL1C = p_gain_normal*err) >> 8.
|
|
* Disasm:
|
|
* LD RW1C, [0x0454] ; p_gain_normal (cal+0x118 default +480)
|
|
* MUL RL1C, err ; RL1C = p_gain_normal * err (signed)
|
|
* SHRAL RL1C, #0x8 ; arithmetic shift right 8 (32-bit)
|
|
* ST RW1C, DAT_02b8 ; pi_p_term = lo16
|
|
*/
|
|
int32_t comp32 = MUL_S16(rt->p_gain_normal, error);
|
|
int16_t comp = (int16_t)(shra32(comp32, 8) & 0xFFFF);
|
|
rt->pi_p_term = comp;
|
|
|
|
/* [0x677a-0x677f] active_request = pi_p_term + pi_integ_hi */
|
|
int16_t combined = (int16_t)(comp + rt->pi_integ_hi);
|
|
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 >
|
|
* pi_high_clamp_ceiling, pin to ceiling 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->pi_high_clamp_ceiling) {
|
|
rt->active_request = rt->pi_high_clamp_ceiling;
|
|
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, cl_gate_input 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.cl_gate_input < 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. P-shape bounds at
|
|
* DAT_0450 / DAT_0452 are set once at boot by FUN_76aa — see
|
|
* docs/open-questions.md §2 closeout. */
|
|
if (err > rt->p_shape_bound_pos) {
|
|
rt->active_request = cal->pi_high_clamp;
|
|
rt->pi_flag_338 = 0x01;
|
|
} else if (err < rt->p_shape_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] pi_high_clamp_ceiling = setpoint_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] */
|
|
}
|