544 lines
26 KiB
C
544 lines
26 KiB
C
/**
|
||
* @file pwm.c
|
||
* @brief Compact single-file implementation of the VP44 PWM solenoid control
|
||
* pipeline. Combines interpolation, setpoint, supervisor, PI, PWM
|
||
* output, orchestrator, ported ex-external routines, and default
|
||
* cal data.
|
||
*
|
||
* Every arithmetic choice (MUL vs MULU, SHRA vs SHR, JGE vs JC) mirrors the
|
||
* MCS-96 assembly — see the original src/*.c files for per-block address
|
||
* annotations and disassembly/*.txt for the source.
|
||
*/
|
||
#include "pwm.h"
|
||
#include "cal_tables_rom.h"
|
||
|
||
/* Forward decls for ported ex-external routines (defined below). */
|
||
static int16_t s_eval (const pwm_submap_descr_t *descr,
|
||
pwm_interp_slot_t *slot);
|
||
static int16_t s_combine (const int16_t *y_base,
|
||
const pwm_interp_slot_t *sx,
|
||
const pwm_interp_slot_t *sy);
|
||
static int16_t s_refine (const int16_t *y_base,
|
||
const pwm_interp_slot_t *slot);
|
||
static void s_correction(pwm_runtime_t *rt,
|
||
const pwm_calibration_t *cal);
|
||
static void s_recovery (pwm_runtime_t *rt,
|
||
const pwm_calibration_t *cal,
|
||
uint16_t rpm);
|
||
|
||
/* ═════════════════════════════════════════════════════════════════════
|
||
* Runtime init (file-static — exposed via pwm_init)
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
static void runtime_reset(pwm_runtime_t *rt)
|
||
{
|
||
memset(rt, 0, sizeof *rt);
|
||
rt->mode_flags = 0x03; /* first_call + outer */
|
||
rt->min_rpm_openloop = 0x01A4; /* 420 RPM */
|
||
rt->upper_breakpoint = 107;
|
||
rt->lower_breakpoint = (int16_t)0xFF95;
|
||
rt->center_slope = 2560;
|
||
rt->upper_outer_slope = 5120;
|
||
rt->lower_outer_slope = 5120;
|
||
rt->upper_integrator_gain = 1024;
|
||
rt->center_integrator_gain = 1024;
|
||
rt->lower_integrator_gain = 2048;
|
||
rt->pwm_period = 29851;
|
||
rt->shape_scale = 24;
|
||
rt->pwm_min = 205;
|
||
rt->pwm_max = 3890;
|
||
/* Observed defaults from Ghidra annotations on compute_closed_loop_correction
|
||
* (0x01F4, 0x028A) and fast_recovery (0x01F4). TODO: locate flash source. */
|
||
rt->pos_error_normalizer = 500; /* 0x01F4 */
|
||
rt->neg_error_normalizer = 650; /* 0x028A */
|
||
rt->recovery_count_threshold = 500; /* 0x01F4 */
|
||
}
|
||
|
||
static void apply_calibration(pwm_runtime_t *rt, const pwm_calibration_t *c)
|
||
{
|
||
rt->upper_breakpoint = c->pi_upper_breakpoint;
|
||
rt->lower_breakpoint = c->pi_lower_breakpoint;
|
||
rt->center_slope = c->pi_center_slope;
|
||
rt->upper_outer_slope = c->pi_upper_outer_slope;
|
||
rt->lower_outer_slope = c->pi_lower_outer_slope;
|
||
rt->upper_integrator_gain = c->pi_upper_integrator_gain;
|
||
rt->center_integrator_gain = c->pi_center_integrator_gain;
|
||
rt->lower_integrator_gain = c->pi_lower_integrator_gain;
|
||
rt->setpoint_offset = c->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)
|
||
{
|
||
runtime_reset(rt);
|
||
apply_calibration(rt, cal);
|
||
rt->bound_cal = cal;
|
||
rt->bound_flash = flash;
|
||
rt->bound_getters = getters;
|
||
/* Resolve each descriptor's input_ptr into this rt's inputs block, so
|
||
* s_eval() can dereference live values. Descriptor order/IDs are fixed
|
||
* by the PWM_SUBMAP_* enum in cal_tables_rom.h. */
|
||
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.state_130 = g->state_130 (ctx);
|
||
rt->inputs.supply_voltage = g->supply_voltage(ctx);
|
||
rt->inputs.temperature = g->temperature (ctx);
|
||
}
|
||
|
||
/* ═════════════════════════════════════════════════════════════════════
|
||
* Interpolation — helper_from_cal (0x838b–0x83fa)
|
||
* Descending-X piecewise linear, signed MUL + signed DIV.
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
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];
|
||
|
||
/* One-step binary midpoint jump, then linear scan. */
|
||
uint16_t mid = (n & 0xFFFEu) / 2u;
|
||
uint16_t k = (in < x[mid]) ? (uint16_t)(mid + 1u) : 1u;
|
||
while (k < n && in < x[k]) 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);
|
||
return (int16_t)((int16_t)(prod / (int32_t)dx) + y[k]);
|
||
}
|
||
|
||
/* ═════════════════════════════════════════════════════════════════════
|
||
* Setpoint — precompute_control_support_maps (0x8e36–0x8f1f)
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
static void setpoint_compute(pwm_runtime_t *rt, const pwm_calibration_t *cal)
|
||
{
|
||
/* Pair 0: slot_x=sp0_A(rpm), slot_y=sp0_B(inj_qty_demand) */
|
||
s_eval(&pwm_submap_descrs[PWM_SUBMAP_SP0_A], &rt->setpoint_slot_x);
|
||
s_eval(&pwm_submap_descrs[PWM_SUBMAP_SP0_B], &rt->setpoint_slot_y);
|
||
rt->compensation_angle =
|
||
s_combine(cal->y_pair[0], &rt->setpoint_slot_x, &rt->setpoint_slot_y);
|
||
|
||
/* Pair 1: slot_x reused (still sp0_A rpm), slot_y=sp1_B(temperature) */
|
||
s_eval(&pwm_submap_descrs[PWM_SUBMAP_SP1_B], &rt->setpoint_slot_y);
|
||
rt->compensation_angle = (int16_t)(rt->compensation_angle +
|
||
s_combine(cal->y_pair[1], &rt->setpoint_slot_x, &rt->setpoint_slot_y));
|
||
|
||
/* Pair 2: slot_x=sp2_A(rpm), slot_y=sp2_B(angle_dec_cmd) */
|
||
s_eval(&pwm_submap_descrs[PWM_SUBMAP_SP2_A], &rt->setpoint_slot_x);
|
||
s_eval(&pwm_submap_descrs[PWM_SUBMAP_SP2_B], &rt->setpoint_slot_y);
|
||
rt->compensation_angle = (int16_t)(rt->compensation_angle +
|
||
s_combine(cal->y_pair[2], &rt->setpoint_slot_x, &rt->setpoint_slot_y));
|
||
|
||
int16_t sum = (int16_t)(rt->inputs.b_fb_kw + rt->compensation_angle);
|
||
int16_t half = shra16(sum, 1); /* SHRA — signed halve */
|
||
rt->pre_offset_target = half;
|
||
|
||
int16_t t = (int16_t)(half + rt->setpoint_offset);
|
||
if (t < cal->target_minimum) t = cal->target_minimum;
|
||
rt->target = t;
|
||
}
|
||
|
||
/* ═════════════════════════════════════════════════════════════════════
|
||
* Supervisor — update_closed_loop_supervisor (0x929b–0x92f1)
|
||
* One-sided (upper-only) clamp on cl_error_delta.
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
static void supervisor_update(pwm_runtime_t *rt)
|
||
{
|
||
if (rt->reset_flag == 1) {
|
||
rt->reset_flag = 0;
|
||
rt->cl_enable_counter = 0;
|
||
rt->supervisor_state_17e = 0;
|
||
/* ROM snapshots integrator_hi → 0x033e here; confirmed dead-store, dropped */
|
||
} else {
|
||
rt->cl_enable_counter = (int16_t)(rt->cl_enable_counter + 1);
|
||
}
|
||
int16_t err = (int16_t)(rt->target - rt->inputs.ckp_in);
|
||
err = (int16_t)(err + rt->angle_error_shaped);
|
||
rt->cl_error_delta = err;
|
||
if (err > rt->max_cl_error) rt->cl_error_delta = rt->max_cl_error;
|
||
}
|
||
|
||
/* ═════════════════════════════════════════════════════════════════════
|
||
* PI controller — finalize_angle_request (0x713b–0x7414)
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
static void shape_error(pwm_runtime_t *rt)
|
||
{
|
||
int16_t e = rt->angle_error;
|
||
int16_t bp, slope, gain;
|
||
|
||
if (e > rt->upper_breakpoint) {
|
||
rt->mode_flags |= MODE_OUTER_REGION;
|
||
bp = rt->upper_breakpoint; slope = rt->upper_outer_slope;
|
||
gain = rt->upper_integrator_gain;
|
||
} else if (e < rt->lower_breakpoint) {
|
||
rt->mode_flags |= MODE_OUTER_REGION;
|
||
bp = rt->lower_breakpoint; slope = rt->lower_outer_slope;
|
||
gain = rt->lower_integrator_gain;
|
||
} else {
|
||
rt->mode_flags &= (uint8_t)~MODE_OUTER_REGION;
|
||
rt->integrator_gain = rt->center_integrator_gain;
|
||
rt->angle_error_shaped =
|
||
(int16_t)shra32(MUL_S16(rt->center_slope, e), 8);
|
||
return;
|
||
}
|
||
rt->integrator_gain = gain;
|
||
int32_t outer = MUL_S16((int16_t)(e - bp), slope);
|
||
int32_t center = MUL_S16(rt->center_slope, bp);
|
||
rt->angle_error_shaped = (int16_t)shra32(outer + center, 8);
|
||
}
|
||
|
||
static void pi_controller_update(pwm_runtime_t *rt,
|
||
const pwm_calibration_t *cal,
|
||
const pwm_flash_t *flash)
|
||
{
|
||
const uint16_t rpm = rt->inputs.rpm;
|
||
const int16_t inj_qty_demand = rt->inputs.inj_qty_demand;
|
||
|
||
/* A. Entry conditions — all three must hold for closed-loop. */
|
||
bool open_loop = (rt->system_flags_110 & 0x20) != 0
|
||
|| rt->inputs.state_130 < 0
|
||
|| (uint16_t)rpm < (uint16_t)rt->min_rpm_openloop;
|
||
|
||
if (open_loop) {
|
||
rt->mode_flags |= MODE_FIRST_CALL;
|
||
rt->active_request = rt->target;
|
||
if (rt->active_request < flash->request_upper_limit)
|
||
rt->active_request = flash->request_upper_limit;
|
||
goto rotate_flags;
|
||
}
|
||
|
||
/* C. Error */
|
||
rt->angle_error = (int16_t)(rt->target - rt->estimated_angle);
|
||
|
||
/* D/E/F. Large-positive / large-negative / normal-range paths */
|
||
if (rt->angle_error > flash->large_pos_error_thresh) {
|
||
rt->mode_flags = (uint8_t)((rt->mode_flags | MODE_LARGE_POS) & ~MODE_LARGE_NEG);
|
||
s_recovery(rt, cal, rpm);
|
||
} else if (rt->angle_error < flash->large_neg_error_thresh) {
|
||
rt->mode_flags = (uint8_t)((rt->mode_flags | MODE_LARGE_NEG) & ~MODE_LARGE_POS);
|
||
if ((uint16_t)inj_qty_demand > (uint16_t)flash->inj_qty_demand_thresh)
|
||
s_recovery(rt, cal, rpm);
|
||
else
|
||
rt->recovery_counter = 0;
|
||
} else {
|
||
rt->mode_flags &= (uint8_t)~(MODE_LARGE_POS | MODE_LARGE_NEG);
|
||
if ((uint16_t)rt->error_persist_counter > 0u)
|
||
rt->error_persist_counter = (int16_t)(rt->error_persist_counter - 1);
|
||
if (rt->error_persist_counter == 0)
|
||
rt->system_flags_110 &= (uint8_t)~0x01;
|
||
if (rt->recovery_counter > 0)
|
||
rt->recovery_counter = (int16_t)(rt->recovery_counter - 1);
|
||
}
|
||
|
||
/* G. Shape error */
|
||
shape_error(rt);
|
||
|
||
/* H. Integrator — first-call init or anti-windup-gated accumulate */
|
||
if (rt->mode_flags & MODE_FIRST_CALL) {
|
||
int16_t lo_prod = (int16_t)MUL_S16(rt->first_call_gain, rt->angle_error);
|
||
int16_t init_hi = (int16_t)(shra16(lo_prod, 4) + rt->inputs.ckp_in);
|
||
rt->integrator_state = (int32_t)(((uint32_t)(uint16_t)init_hi << 16) |
|
||
INTEG_LO(rt->integrator_state));
|
||
rt->mode_flags &= (uint8_t)~MODE_FIRST_CALL;
|
||
} else {
|
||
bool freeze = (rt->angle_error < 0)
|
||
? (rt->mode_flags & MODE_NEG_SAT) != 0
|
||
: (rt->mode_flags & MODE_POS_SAT) != 0;
|
||
if (!freeze) {
|
||
int32_t inc = MUL_S16(rt->angle_error, rt->integrator_gain) << 4;
|
||
rt->integrator_state += inc;
|
||
}
|
||
}
|
||
|
||
/* I. Output + saturation clamps (signed) */
|
||
rt->active_request = (int16_t)(rt->angle_error_shaped +
|
||
INTEG_HI(rt->integrator_state));
|
||
if (rt->active_request < flash->request_upper_limit) {
|
||
rt->active_request = flash->request_upper_limit;
|
||
rt->mode_flags = (uint8_t)((rt->mode_flags | MODE_NEG_SAT) & ~MODE_POS_SAT);
|
||
} else if (rt->active_request > rt->max_cl_error) {
|
||
rt->active_request = rt->max_cl_error;
|
||
rt->mode_flags = (uint8_t)((rt->mode_flags | MODE_POS_SAT) & ~MODE_NEG_SAT);
|
||
} else {
|
||
rt->mode_flags &= (uint8_t)~(MODE_NEG_SAT | MODE_POS_SAT);
|
||
}
|
||
|
||
rotate_flags:
|
||
/* J. Rotate bits 4-5 into bits 6-7 (prev_sat_shadow). Consumed next
|
||
* cycle by fast_recovery (FUN_70ae at 0x70ae/0x70b6) via the pattern
|
||
* sustained = (mode_flags << 2) & mode_flags
|
||
* to detect two-cycle sustained saturation. [0x73f4–0x7414] */
|
||
rt->mode_flags = (uint8_t)((rt->mode_flags & 0x3Fu) |
|
||
(((uint16_t)rt->mode_flags << 2) & 0xC0u));
|
||
}
|
||
|
||
/* ═════════════════════════════════════════════════════════════════════
|
||
* PWM output — compute_pwm_duty_from_maps (0x7415–0x7760)
|
||
* Nearly all UNSIGNED arithmetic (MULU, SHR, DIVU, JC/JNH).
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
static bool rpm_in_any_window(uint16_t rpm, const uint16_t *bp,
|
||
uint16_t lo_pad, uint16_t hi_pad)
|
||
{
|
||
for (int i = 0; i < 4; i++) {
|
||
uint16_t lo = (uint16_t)(bp[2 * i] - lo_pad);
|
||
uint16_t hi = (uint16_t)(bp[2 * i + 1] + hi_pad);
|
||
if (rpm > lo && rpm < hi) return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
static void pwm_output_compute(pwm_runtime_t *rt, const pwm_flash_t *flash)
|
||
{
|
||
const uint16_t rpm = rt->inputs.rpm;
|
||
|
||
/* A. Base duty from submap pair — slot_x=pwm_A(rpm),
|
||
* slot_y=pwm_B(active_request) [0x0046 = PI output feed-forward] */
|
||
s_eval(&pwm_submap_descrs[PWM_SUBMAP_PWM_A], &rt->pwm_slot_x);
|
||
s_eval(&pwm_submap_descrs[PWM_SUBMAP_PWM_B], &rt->pwm_slot_y);
|
||
rt->pwm_duty = (uint16_t)s_combine(
|
||
flash->pwm_y_table, &rt->pwm_slot_x, &rt->pwm_slot_y);
|
||
|
||
/* B. Duty status flag (unsigned) */
|
||
rt->pwm_status_flag = (rt->pwm_duty < 0x29u) ? 1u
|
||
: (rt->pwm_duty > 0xFD7u) ? 2u : 0u;
|
||
|
||
/* C/D. Coarse RPM window — shape_intermediate = +shape_scale if in any */
|
||
rt->shape_scale = flash->shape_scale_src;
|
||
bool coarse = rpm_in_any_window(rpm, flash->rpm_breakpoints, 0, 0);
|
||
|
||
if (coarse) {
|
||
rt->shape_intermediate = rt->shape_scale;
|
||
} else {
|
||
/* E. Detail pass — expanded windows padded by rpm_values[0] */
|
||
uint16_t pad = flash->rpm_values[0];
|
||
bool detail = rpm_in_any_window(rpm, flash->rpm_breakpoints, pad, pad);
|
||
if (!detail && rt->pwm_period < flash->period_max_limit)
|
||
rt->shape_intermediate = (int16_t)(-rt->shape_scale);
|
||
/* else: shape_intermediate unchanged */
|
||
}
|
||
|
||
/* F. Period adjust + unsigned clamp to [min, max] */
|
||
rt->pwm_period = (uint16_t)(rt->pwm_period - (uint16_t)rt->shape_intermediate);
|
||
if (rt->pwm_period > flash->period_max_limit) rt->pwm_period = flash->period_max_limit;
|
||
if (rt->pwm_period < flash->period_min_limit) rt->pwm_period = flash->period_min_limit;
|
||
|
||
/* G. Shape refinement (signed clamp to [0, 0x199]) — shape_eval(supply_voltage) */
|
||
s_eval(&pwm_submap_descrs[PWM_SUBMAP_SHAPE_EVAL], &rt->pwm_slot_x);
|
||
rt->shape_output = s_refine(flash->shape_y_table, &rt->pwm_slot_x);
|
||
if (rt->shape_output < 0) rt->shape_output = 0;
|
||
if (rt->shape_output > (int16_t)0x199) rt->shape_output = (int16_t)0x199;
|
||
|
||
/* H. Duty refinement — all UNSIGNED (SHR, MULU, DIVU) */
|
||
uint16_t range = (uint16_t)((uint16_t)(flash->period_max_limit
|
||
- flash->period_min_limit) >> 8);
|
||
rt->shape_scale = (int16_t)range;
|
||
uint16_t delta = (uint16_t)((uint16_t)(flash->period_max_limit
|
||
- rt->pwm_period) >> 8);
|
||
uint32_t prod = MULU_U16(delta, (uint16_t)rt->shape_output);
|
||
uint16_t trunc = (uint16_t)(prod & 0xFFFFu); /* assembly CLR RW1E */
|
||
uint16_t quot = range ? (uint16_t)(trunc / range) : 0u;
|
||
rt->pwm_duty = (uint16_t)(rt->pwm_duty + quot);
|
||
|
||
/* I. Absolute duty clamp (unsigned) */
|
||
if (rt->pwm_duty < rt->pwm_min) rt->pwm_duty = rt->pwm_min;
|
||
if (rt->pwm_duty > rt->pwm_max) rt->pwm_duty = rt->pwm_max;
|
||
|
||
/* J. HW register split — critical section in assembly (DI/EI) */
|
||
uint32_t hw = MULU_U16(rt->pwm_period, rt->pwm_duty);
|
||
rt->pwm_on_time = (uint16_t)(hw / 0xFFFu);
|
||
rt->pwm_off_time = (uint16_t)(rt->pwm_period - rt->pwm_on_time);
|
||
}
|
||
|
||
/* ═════════════════════════════════════════════════════════════════════
|
||
* Orchestrator — main_pwm_control_service (0x8cc9–0x8cf1)
|
||
* + inlined publish_closed_loop_request (0x937d–0x938f)
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
void pwm_service(pwm_runtime_t *rt)
|
||
{
|
||
/* 0. Refresh external inputs via user getters. */
|
||
read_inputs(rt);
|
||
|
||
const pwm_calibration_t *cal = rt->bound_cal;
|
||
const pwm_flash_t *flash = rt->bound_flash;
|
||
|
||
/* 1. Max-error interpolation */
|
||
if (flash->max_error_count > 0)
|
||
rt->max_cl_error = pwm_interp_lookup(flash->max_error_x,
|
||
flash->max_error_y,
|
||
flash->max_error_count,
|
||
(int16_t)rt->inputs.rpm);
|
||
/* 2. Target setpoint */
|
||
setpoint_compute(rt, cal);
|
||
|
||
/* 3. Supervisor */
|
||
supervisor_update(rt);
|
||
|
||
/* 4. Publish correction + angle estimate */
|
||
s_correction(rt, cal);
|
||
rt->estimated_angle = (int16_t)(rt->inputs.ckp_in + rt->angle_offset);
|
||
|
||
/* 5. PI controller */
|
||
pi_controller_update(rt, cal, flash);
|
||
|
||
/* 6. PWM duty + HW registers */
|
||
pwm_output_compute(rt, flash);
|
||
}
|
||
|
||
/* ═════════════════════════════════════════════════════════════════════
|
||
* External function impls (ported 1:1 from disassembly)
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
/* real_eval_submap (disassembled 81db–8236). */
|
||
static int16_t s_eval(const pwm_submap_descr_t *descr, pwm_interp_slot_t *slot)
|
||
{
|
||
int16_t input_val = (descr && descr->input_ptr) ? *descr->input_ptr : 0;
|
||
uint16_t count = descr ? descr->count : 0u;
|
||
const int16_t *x = descr ? descr->x : NULL;
|
||
if (count == 0u || x == NULL) {
|
||
memset(slot, 0, sizeof *slot);
|
||
return 0;
|
||
}
|
||
slot->row_stride = (int16_t)(count * 2u);
|
||
uint16_t k;
|
||
for (k = 1; k < count; k++) {
|
||
if (input_val >= x[k]) break;
|
||
}
|
||
if (k == 1 && input_val >= x[0]) {
|
||
slot->x_interval = 2; slot->x_offset = 2; slot->y_byte_off = 2;
|
||
return 0;
|
||
}
|
||
if (k >= count) k = (uint16_t)(count - 1u);
|
||
slot->x_interval = (int16_t)(x[k - 1] - x[k]);
|
||
slot->x_offset = (int16_t)(input_val - x[k]);
|
||
slot->y_byte_off = (int16_t)(k * 2u);
|
||
return 0;
|
||
}
|
||
|
||
/* real_combine_submaps (disassembled 8258–82b4). Bilinear over row-major
|
||
* [B_count][A_count] int16 Y-table. slot_x->row_stride is A-axis byte stride. */
|
||
static int16_t s_combine(const int16_t *y_base,
|
||
const pwm_interp_slot_t *sx,
|
||
const pwm_interp_slot_t *sy)
|
||
{
|
||
if (y_base == NULL || sx->x_interval == 0 || sy->x_interval == 0) return 0;
|
||
int32_t row_off = MUL_S16(sy->y_byte_off, sx->row_stride) / 2;
|
||
const int16_t *yp = (const int16_t *)((const uint8_t *)y_base
|
||
+ row_off + sx->y_byte_off);
|
||
int16_t y_here = *yp;
|
||
int16_t y_prev = *(const int16_t *)((const uint8_t *)yp - 2);
|
||
int32_t diff_a = MUL_S16((int16_t)(y_prev - y_here), sx->x_offset);
|
||
int16_t rowB = (int16_t)(y_here + (int32_t)(diff_a / (int32_t)sx->x_interval));
|
||
|
||
const int16_t *yp_p = (const int16_t *)((const uint8_t *)yp - sx->row_stride);
|
||
int16_t y_here_p = *yp_p;
|
||
int16_t y_prev_p = *(const int16_t *)((const uint8_t *)yp_p - 2);
|
||
int32_t diff_a_p = MUL_S16((int16_t)(y_prev_p - y_here_p), sx->x_offset);
|
||
int16_t rowBp = (int16_t)(y_here_p
|
||
+ (int32_t)(diff_a_p / (int32_t)sx->x_interval));
|
||
|
||
int32_t diff_b = MUL_S16((int16_t)(rowBp - rowB), sy->x_offset);
|
||
return (int16_t)(rowB + (int32_t)(diff_b / (int32_t)sy->x_interval));
|
||
}
|
||
|
||
/* compute_closed_loop_correction (disassembled 92f2–9339). */
|
||
static void s_correction(pwm_runtime_t *rt, const pwm_calibration_t *cal)
|
||
{
|
||
int16_t correction = 0;
|
||
if ((uint8_t)(rt->cl_enable_counter & 0xFF) != 0) {
|
||
int16_t normalizer = (rt->cl_error_delta > 0)
|
||
? rt->pos_error_normalizer
|
||
: rt->neg_error_normalizer;
|
||
int32_t product = MUL_S16(rt->cl_error_delta,
|
||
cal->closed_loop_gain_const);
|
||
correction = (int16_t)((product << 4) / (int32_t)normalizer);
|
||
}
|
||
rt->cl_correction_raw = correction;
|
||
rt->supervisor_state_17e = (int16_t)(rt->supervisor_state_17e + correction);
|
||
rt->angle_offset = shra16(rt->supervisor_state_17e, 4);
|
||
}
|
||
|
||
/* real_refine_submap (disassembled 8237–8257). 1D variant of combine. */
|
||
static int16_t s_refine(const int16_t *y_base, const pwm_interp_slot_t *slot)
|
||
{
|
||
if (y_base == NULL || slot->x_interval == 0) return 0;
|
||
const int16_t *yp = (const int16_t *)((const uint8_t *)y_base + slot->y_byte_off);
|
||
int16_t y_here = *yp;
|
||
int16_t y_prev = *(const int16_t *)((const uint8_t *)yp - 2);
|
||
int32_t diff = MUL_S16((int16_t)(y_prev - y_here), slot->x_offset);
|
||
return (int16_t)(y_here + (int32_t)(diff / (int32_t)slot->x_interval));
|
||
}
|
||
|
||
/* fast_recovery FUN_70ae (disassembled 70ae–713a). */
|
||
static void s_recovery(pwm_runtime_t *rt,
|
||
const pwm_calibration_t *cal,
|
||
uint16_t rpm)
|
||
{
|
||
uint16_t mf = (uint16_t)rt->mode_flags;
|
||
uint16_t sustained = (uint16_t)(((mf << 2) & mf));
|
||
if (sustained > 0x30u) {
|
||
if ((uint16_t)rt->recovery_counter
|
||
>= (uint16_t)rt->recovery_count_threshold) {
|
||
rt->system_flags_110 |= 0x01u;
|
||
rt->recovery_counter = 0;
|
||
rt->error_persist_counter = cal->error_persist_init_count;
|
||
} else if ((int16_t)rpm > cal->recovery_rpm_threshold
|
||
&& (rt->system_flags_110 & 0x10u) == 0u
|
||
&& (rt->system_flags_110 & 0x20u) == 0u
|
||
&& rt->error_persist_counter == 0) {
|
||
rt->recovery_counter = (int16_t)(rt->recovery_counter + 1);
|
||
}
|
||
} else if ((rt->system_flags_110 & 0x01u) == 0u) {
|
||
rt->recovery_counter = 0;
|
||
}
|
||
}
|
||
|
||
/* ═════════════════════════════════════════════════════════════════════
|
||
* Default calibration (TODO-placeholder values)
|
||
* ═════════════════════════════════════════════════════════════════════ */
|
||
const pwm_calibration_t pwm_cal_default = {
|
||
.target_minimum = 0,
|
||
.y_pair = { NULL, NULL, NULL },
|
||
.pi_upper_breakpoint = 107,
|
||
.pi_lower_breakpoint = (int16_t)0xFF95,
|
||
.pi_center_slope = 2560,
|
||
.pi_upper_outer_slope = 5120,
|
||
.pi_lower_outer_slope = 5120,
|
||
.pi_upper_integrator_gain = 1024,
|
||
.pi_center_integrator_gain = 1024,
|
||
.pi_lower_integrator_gain = 2048,
|
||
.setpoint_offset = 0, /* CAL+0x52 - CAL+0x54 — populated from ROM */
|
||
};
|
||
|
||
const pwm_flash_t pwm_flash_default = {
|
||
.max_error_x = {0},
|
||
.max_error_y = {0},
|
||
.max_error_count = 1,
|
||
.max_error_input_addr = 0,
|
||
.request_upper_limit = 0,
|
||
.large_pos_error_thresh = 0x200,
|
||
.large_neg_error_thresh = (int16_t)0xFE00,
|
||
.inj_qty_demand_thresh = 0,
|
||
.rpm_breakpoints = {0},
|
||
.rpm_values = {0},
|
||
.shape_scale_src = 24,
|
||
.period_max_limit = 0x8000,
|
||
.period_min_limit = 0x6000,
|
||
.pwm_y_table = NULL,
|
||
.shape_y_table = NULL,
|
||
};
|
||
|
||
/* ROM-extracted pwm_cal_rom, pwm_flash_rom, pwm_submap_descrs[], and all
|
||
* Y-table / submap-x static payloads live in cal_tables_rom.{h,c} — both
|
||
* auto-generated by tools/extract_calibration.py --target compact. Link
|
||
* cal_tables_rom.c alongside pwm.c to get them. */
|