677 lines
30 KiB
C
677 lines
30 KiB
C
/**
|
||
* @file pwm.c (families/T06215/compact_src)
|
||
* @brief Compact single-file implementation of the T06215 PWM control
|
||
* pipeline.
|
||
*
|
||
* Pipeline (pwm_service @ 0x7780):
|
||
* 0. setpoint interp (FUN_7051) — writes pi_high_clamp_ceiling (DAT_0344)
|
||
* 1. supervisor (s_supervisor) — reset + counter + error + clamp
|
||
* 2. publish_cl (s_publish_cl) — calls cl_correction, publishes est_angle
|
||
* 3. pi_update (s_pi_update @ 0x542f) — open/closed-loop PI body
|
||
* 4. pwm_output (s_pwm_output) — eval+eval+combine+saturate+HW shadow
|
||
*
|
||
* The PI block (s_pi_update + s_recovery) was re-translated 1:1 from the
|
||
* t06215 disasm at 0x542f / 0x53a2 on 2026-04-29. Earlier versions of this
|
||
* file inherited a t06211-shaped body (separate s_pi_compensation /
|
||
* s_pi_integrator_step / pi_flag_338 / pi_flag_c7) whose cited addresses
|
||
* (0x67c4, 0x66ad, 0x672b, 0x7c85) do not exist in this binary.
|
||
*/
|
||
#include "pwm.h"
|
||
|
||
/* Forward decls. */
|
||
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);
|
||
static void s_recovery (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. */
|
||
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->p_slope_large_pos = cal->init_p_slope_large_pos;
|
||
rt->p_slope_large_neg = cal->init_p_slope_large_neg;
|
||
rt->integ_step_large_pos = cal->init_integ_step_large_pos;
|
||
rt->integ_step_large_neg = cal->init_integ_step_large_neg;
|
||
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;
|
||
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);
|
||
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);
|
||
|
||
rt->can_raw_b_fb_kw = rt->inputs.b_fb_kw;
|
||
}
|
||
|
||
/* ═════════════════════════════════════════════════════════════════════
|
||
* Interpolation (FUN_7168, fingerprint #3)
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
|
||
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);
|
||
return (int16_t)(quot + y[k]);
|
||
}
|
||
|
||
/* ═════════════════════════════════════════════════════════════════════
|
||
* Submap eval / bilinear combine / refine
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
|
||
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]) {
|
||
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));
|
||
}
|
||
|
||
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 0 — Setpoint interp (pwm_service:0x7780 → FUN_7051(cal+0x142))
|
||
* Writes pi_high_clamp_ceiling (DAT_0344 in ROM).
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
|
||
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 0b — CAN-decoded setpoint (FUN_64c3 family)
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
|
||
static void s_setpoint_can_decode(pwm_runtime_t *rt,
|
||
const pwm_calibration_t *cal)
|
||
{
|
||
int16_t raw = rt->can_raw_b_fb_kw;
|
||
if (raw > cal->b_fb_kw_upper_bound) return;
|
||
if (raw < cal->b_fb_kw_lower_bound) return;
|
||
|
||
int16_t half = shra16(raw, 1);
|
||
rt->can_half_12a = half;
|
||
|
||
int16_t result = (int16_t)(half + rt->setpoint_offset + rt->rw42_state);
|
||
if (result < cal->target_5e_min_clamp) {
|
||
result = cal->target_5e_min_clamp;
|
||
}
|
||
rt->target_5e = result;
|
||
}
|
||
|
||
/* ═════════════════════════════════════════════════════════════════════
|
||
* Stage 1 — Supervisor (s_supervisor @ 0x7d26)
|
||
*
|
||
* 1:1 re-translation 2026-04-29. Earlier port body was inherited from
|
||
* t06211 and used `compensation_angle` (no t06215 producer) and a
|
||
* `b_fb_kw_baseline` reset stash (no t06215 binding). The actual t06215
|
||
* supervisor adds the **PI P-term** (DAT_0276) and stashes the
|
||
* integrator high word into DAT_02ec on reset (a dead store, but kept
|
||
* here for parity).
|
||
*
|
||
* Disasm:
|
||
* 7d26 LDBZE pi_shape_flag-adjacent (R2E = reset_flag)
|
||
* 7d2b CMP #1 ; JNE LAB_7d4c
|
||
* ; --- reset path ---
|
||
* 7d31 reset_flag = 0
|
||
* 7d36 cl_enable_counter (DAT_02ee) = 0
|
||
* 7d3b supervisor_state (RW17E) = 0
|
||
* 7d40-7d45 DAT_02ec = pi_integ_hi (DAT_028a snapshot — dead store)
|
||
* 7d4a SJMP LAB_7d5b
|
||
* ; --- non-reset path ---
|
||
* LAB_7d4c LD RW1C, cl_enable_counter; RW1E = RW1C+1; ST cl_enable_counter
|
||
* ; --- common publish ---
|
||
* LAB_7d5b SUB RW1C, RW5E, ckp_in
|
||
* ADD RW1C, pi_p_term (DAT_0276)
|
||
* ST angle_error_raw (DAT_02f0), RW1C
|
||
* CMP RW1C, pi_high_clamp_ceiling (DAT_0344)
|
||
* JLE exit
|
||
* angle_error_raw = pi_high_clamp_ceiling
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
|
||
static void s_supervisor(pwm_runtime_t *rt)
|
||
{
|
||
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 {
|
||
rt->cl_enable_counter = (uint16_t)(rt->cl_enable_counter + 1u);
|
||
}
|
||
|
||
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;
|
||
|
||
if (error > rt->pi_high_clamp_ceiling) {
|
||
rt->angle_error_raw = rt->pi_high_clamp_ceiling;
|
||
}
|
||
}
|
||
|
||
/* ═════════════════════════════════════════════════════════════════════
|
||
* Stage 2a — Closed-loop correction
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
|
||
static void s_cl_correct(pwm_runtime_t *rt,
|
||
const pwm_calibration_t *cal)
|
||
{
|
||
int16_t correction;
|
||
|
||
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 2 — Publish CL
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
|
||
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 3 — PI controller
|
||
*
|
||
* 1:1 re-translation from t06215 disasm s_pi_update @ 0x542f and
|
||
* s_recovery @ 0x53a2 (live Ghidra session, 2026-04-29). Cross-reference:
|
||
* the trailing byte rotate at LAB_56e8 (56e8–5708) saves bits 4,5 of the
|
||
* current cycle into bits 6,7 for next cycle's s_recovery sustained-band
|
||
* detector.
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
|
||
/* s_recovery @ 0x53a2.
|
||
*
|
||
* Disasm:
|
||
* 53a2 LDBZE RW1C, pi_shape_flag ; zero-extended into 16-bit
|
||
* 53a7 SHL RW1C, #2 ; <<2 (in 16-bit reg)
|
||
* 53aa LDBZE RW1E, pi_shape_flag
|
||
* 53af AND RW1C, RW1E ; (flag<<2) & flag
|
||
* 53b2 CMP RW1C, #0x30
|
||
* 53b6 JNH LAB_5421 ; <= 0x30 → "no sustained" path
|
||
* ; --- Sustained large-error detected (this+previous cycle bit4 or bit5) ---
|
||
* 53b8 LD RW1C, pi_state_118
|
||
* 53bd CMP RW1C, RAM[0x027a] ; RAM[0x027a] = cal+0x112 boot-cached
|
||
* 53c2 JNC LAB_53ea ; counter < threshold → gated-increment
|
||
* ; --- Latch path: counter saturated ---
|
||
* 53c4 system_flags_110 |= 1
|
||
* 53d1 pi_state_118 = 0
|
||
* 53e1 pi_state_c2 = cal[0x114] ; reload cooldown
|
||
* 53e9 RET
|
||
* LAB_53ea: ; counter still < threshold
|
||
* 53f5 CMP rpm, cal[0x126]
|
||
* 53f8 JLE LAB_542e (RET) ; rpm <= threshold → no increment
|
||
* 53fa-5407 if (system_flags_110 & 0x30) RET
|
||
* 540a if pi_state_c2 != 0 RET
|
||
* 5411 pi_state_118 += 1
|
||
* 5420 RET
|
||
* LAB_5421: ; no sustained large-error
|
||
* 5421 if (system_flags_110 & 1) RET ; latched → keep counter
|
||
* 5429 pi_state_118 = 0
|
||
* 542e RET
|
||
*/
|
||
static void s_recovery(pwm_runtime_t *rt,
|
||
const pwm_calibration_t *cal)
|
||
{
|
||
uint16_t flag_u16 = (uint16_t)rt->pi_shape_flag;
|
||
uint16_t fold = (uint16_t)((flag_u16 << 2) & flag_u16);
|
||
|
||
if (fold > 0x30u) {
|
||
/* Sustained large-error detected. */
|
||
if ((uint16_t)rt->pi_state_118 < (uint16_t)cal->pi_sat_count_threshold) {
|
||
/* Counter still below threshold — gated increment. */
|
||
if ((int16_t)rt->inputs.rpm <= cal->rpm_threshold_recovery) return;
|
||
if ((rt->system_flags_110 & 0x10u) != 0u) return;
|
||
if ((rt->system_flags_110 & 0x20u) != 0u) return;
|
||
if (rt->pi_state_c2 != 0) return;
|
||
rt->pi_state_118 = (int16_t)(rt->pi_state_118 + 1);
|
||
} else {
|
||
/* Saturated — latch + reset. */
|
||
rt->system_flags_110 = (uint8_t)(rt->system_flags_110 | 0x01u);
|
||
rt->pi_state_118 = 0;
|
||
rt->pi_state_c2 = cal->pi_state_c2_reload;
|
||
}
|
||
} else {
|
||
/* No sustained large-error this cycle. */
|
||
if ((rt->system_flags_110 & 0x01u) == 0u) {
|
||
rt->pi_state_118 = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
/* s_pi_update @ 0x542f. See doc-comment at top of "Stage 3". */
|
||
static void s_pi_update(pwm_runtime_t *rt,
|
||
const pwm_calibration_t *cal)
|
||
{
|
||
/* ── Phase A — Open-loop gate [0x542f-0x5489] ──
|
||
* Disasm: bit-test R110.5; CMP RW130 < 0; CMP rpm < flash[0x605c]. */
|
||
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 < cal->pi_cl_rpm_floor) open_loop = true;
|
||
|
||
if (open_loop) {
|
||
/* 0x544b-0x5453: pi_shape_flag |= 0x01 */
|
||
rt->pi_shape_flag = (uint8_t)(rt->pi_shape_flag | 0x01u);
|
||
/* 0x5458: DAT_0274 = RW5E */
|
||
rt->pi_preclamp_out = rt->target_5e;
|
||
/* 0x5468-0x5482: if (RW5E < cal[0x128]) DAT_0274 = cal[0x128] */
|
||
if (rt->target_5e < cal->pi_low_clamp) {
|
||
rt->pi_preclamp_out = cal->pi_low_clamp;
|
||
}
|
||
goto trailing_rotate;
|
||
}
|
||
|
||
/* ── Phase B — Error-band classify [0x5489-0x554e] ──
|
||
* Disasm 0x5489: SUB RW1C, RW5E, DAT_02cc → DAT_0278. */
|
||
int16_t err = (int16_t)(rt->target_5e - rt->estimated_angle);
|
||
rt->angle_error_pi = err;
|
||
|
||
if (err > cal->large_pos_error_thresh) {
|
||
/* 0x54a4-0x54af: flag = (flag | 0x10) & 0xDF */
|
||
rt->pi_shape_flag = (uint8_t)((rt->pi_shape_flag | 0x10u) & 0xDFu);
|
||
s_recovery(rt, cal);
|
||
} else if (err < cal->large_neg_error_thresh) {
|
||
/* 0x54cd-0x54d8: flag = (flag | 0x20) & 0xEF */
|
||
rt->pi_shape_flag = (uint8_t)((rt->pi_shape_flag | 0x20u) & 0xEFu);
|
||
/* 0x54dd-0x54f9: JH check is unsigned; matches CMP RW44, cal[0x116]. */
|
||
if ((uint16_t)rt->inputs.inj_qty_demand > (uint16_t)cal->inj_qty_thresh) {
|
||
s_recovery(rt, cal);
|
||
} else {
|
||
rt->pi_state_118 = 0;
|
||
}
|
||
} else {
|
||
/* 0x54fb-0x554e: flag &= 0xCF; decrement c2 (clear latch when 0); decrement 118 */
|
||
rt->pi_shape_flag = (uint8_t)(rt->pi_shape_flag & 0xCFu);
|
||
if ((uint16_t)rt->pi_state_c2 != 0u) {
|
||
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);
|
||
}
|
||
}
|
||
|
||
/* ── Phase C — P-shape into pi_p_term / pi_p_gain_active [0x554e-0x560c] ── */
|
||
int32_t pterm32; /* 32-bit accumulator for the SHRAL (>>8) */
|
||
if (err > rt->p_shape_bound_pos) {
|
||
/* 0x555a-0x558f: large-positive arm. */
|
||
rt->pi_shape_flag = (uint8_t)(rt->pi_shape_flag | 0x02u);
|
||
rt->pi_p_gain_active = rt->integ_step_large_pos; /* DAT_0286 = DAT_0282 */
|
||
int32_t slope_part = MUL_S16((int16_t)(err - rt->p_shape_bound_pos),
|
||
rt->p_slope_large_pos);
|
||
int32_t anchor_part = MUL_S16(rt->p_gain_normal, rt->p_shape_bound_pos);
|
||
pterm32 = slope_part + anchor_part;
|
||
rt->pi_p_term = (int16_t)(shra32(pterm32, 8) & 0xFFFF);
|
||
} else if (err < rt->p_shape_bound_neg) {
|
||
/* 0x559d-0x55cc: large-negative arm. */
|
||
rt->pi_shape_flag = (uint8_t)(rt->pi_shape_flag | 0x02u);
|
||
rt->pi_p_gain_active = rt->integ_step_large_neg; /* DAT_0286 = DAT_0284 */
|
||
int32_t slope_part = MUL_S16((int16_t)(err - rt->p_shape_bound_neg),
|
||
rt->p_slope_large_neg);
|
||
int32_t anchor_part = MUL_S16(rt->p_gain_normal, rt->p_shape_bound_neg);
|
||
pterm32 = slope_part + anchor_part;
|
||
rt->pi_p_term = (int16_t)(shra32(pterm32, 8) & 0xFFFF);
|
||
} else {
|
||
/* 0x55e2-0x5607: normal-range arm. */
|
||
rt->pi_shape_flag = (uint8_t)(rt->pi_shape_flag & 0xFDu);
|
||
rt->pi_p_gain_active = rt->integ_step_normal; /* DAT_0286 = DAT_0456 */
|
||
int32_t prod = MUL_S16(rt->p_gain_normal, err);
|
||
rt->pi_p_term = (int16_t)(shra32(prod, 8) & 0xFFFF);
|
||
}
|
||
|
||
/* ── Phase D — Integrator gate [0x560c-0x5679] ──
|
||
* Disasm 0x560c-0x5611: if pi_shape_flag.bit0 set (open-loop, but we
|
||
* returned earlier in that case) → take fast-path that overrides
|
||
* pi_integ_hi from open_loop_p_gain. We never enter here in the
|
||
* closed-loop path, but preserve the structure.
|
||
* 0x563b onward: closed-loop integrator step with anti-windup. */
|
||
if ((rt->pi_shape_flag & 0x01u) != 0u) {
|
||
/* Open-loop fast-path (defensive — the open-loop early-return
|
||
* above means this branch is unreachable from the closed-loop
|
||
* path; preserved here in case a future cycle enters with bit 0
|
||
* latched from elsewhere). */
|
||
int32_t prod = MUL_S16(rt->open_loop_p_gain, err);
|
||
int16_t shifted = shra16((int16_t)(prod & 0xFFFF), 4);
|
||
rt->pi_integ_hi = (int16_t)(shifted + rt->inputs.ckp_in);
|
||
rt->pi_shape_flag = (uint8_t)(rt->pi_shape_flag & 0xFEu);
|
||
} else {
|
||
/* 0x563b-0x5654: anti-windup gate via previous-cycle clamp bits. */
|
||
bool gate_skip;
|
||
if (err < 0) {
|
||
gate_skip = (rt->pi_shape_flag & 0x04u) != 0u; /* bit 2: clamped low */
|
||
} else {
|
||
gate_skip = (rt->pi_shape_flag & 0x08u) != 0u; /* bit 3: clamped high */
|
||
}
|
||
if (!gate_skip) {
|
||
/* 0x5657-0x5674: 32-bit signed integrator step.
|
||
* step = (err * pi_p_gain_active) << 4
|
||
* {hi:lo} += step
|
||
* MCS-96 SHLL is logical, so cast through unsigned. */
|
||
int32_t prod = MUL_S16(err, rt->pi_p_gain_active);
|
||
int32_t step = (int32_t)((uint32_t)prod << 4);
|
||
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);
|
||
}
|
||
}
|
||
|
||
/* ── Phase E — Combine + clamp [0x5679-0x56e3] ── */
|
||
rt->pi_preclamp_out = (int16_t)(rt->pi_p_term + rt->pi_integ_hi);
|
||
|
||
if (rt->pi_preclamp_out < cal->pi_low_clamp) {
|
||
/* 0x5696-0x56b6: clamp low; flag = (flag & 0xF7) | 4 */
|
||
rt->pi_preclamp_out = cal->pi_low_clamp;
|
||
rt->pi_shape_flag = (uint8_t)((rt->pi_shape_flag & 0xF7u) | 0x04u);
|
||
} else if (rt->pi_preclamp_out > rt->pi_high_clamp_ceiling) {
|
||
/* 0x56c2-0x56d9: clamp high; flag = (flag & 0xFB) | 8 */
|
||
rt->pi_preclamp_out = rt->pi_high_clamp_ceiling;
|
||
rt->pi_shape_flag = (uint8_t)((rt->pi_shape_flag & 0xFBu) | 0x08u);
|
||
} else {
|
||
/* 0x56db-0x56e0: in-range — clear bits 2,3 */
|
||
rt->pi_shape_flag = (uint8_t)(rt->pi_shape_flag & 0xF3u);
|
||
}
|
||
|
||
trailing_rotate:
|
||
/* ── Trailing rotate [LAB_56e8 0x56e8-0x5708] ──
|
||
* Disasm 0x56e8: ST DAT_0274, RW46 ; publish active_request
|
||
* 0x56ed-0x5703: pi_shape_flag = (flag & 0x3F) | ((flag<<2) & 0xC0)
|
||
* Saves this cycle's bits 4,5 into bits 6,7 for the next cycle's
|
||
* s_recovery sustained-band detector.
|
||
*/
|
||
rt->active_request = rt->pi_preclamp_out;
|
||
{
|
||
uint8_t f = rt->pi_shape_flag;
|
||
rt->pi_shape_flag = (uint8_t)((f & 0x3Fu) | (uint8_t)((f << 2) & 0xC0u));
|
||
}
|
||
}
|
||
|
||
/* ═════════════════════════════════════════════════════════════════════
|
||
* Stage 4 — PWM output (FUN_5314 @ 0x5314-0x565f)
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
|
||
static void s_pwm_output(pwm_runtime_t *rt,
|
||
const pwm_calibration_t *cal)
|
||
{
|
||
s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_PWM_A],
|
||
&rt->pwm_slot_a);
|
||
s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_PWM_B],
|
||
&rt->pwm_slot_b);
|
||
int16_t duty = s_combine(cal->pwm_y_table, &rt->pwm_slot_a, &rt->pwm_slot_b);
|
||
rt->pwm_duty = (uint16_t)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;
|
||
|
||
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;
|
||
|
||
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) {
|
||
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;
|
||
}
|
||
if (rpm_s < (int16_t)(hi + halfwidth)) {
|
||
phase3 = 0;
|
||
break;
|
||
}
|
||
}
|
||
if (phase3 && *pwm_period < cal->pwm_period_max) {
|
||
rt->pwm_slew_increment = (int16_t)(-step_mag);
|
||
}
|
||
}
|
||
|
||
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;
|
||
|
||
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;
|
||
|
||
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;
|
||
|
||
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;
|
||
|
||
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;
|
||
|
||
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
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
|
||
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 pwm_service @ 0x7780)
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
|
||
void pwm_service(pwm_runtime_t *rt)
|
||
{
|
||
read_inputs(rt);
|
||
|
||
const pwm_calibration_t *cal = rt->bound_cal;
|
||
|
||
s_setpoint(rt); /* writes pi_high_clamp_ceiling */
|
||
s_setpoint_can_decode(rt, cal);
|
||
s_supervisor(rt);
|
||
s_publish_cl(rt, cal);
|
||
s_pi_update(rt, cal);
|
||
s_pwm_output(rt, cal);
|
||
}
|