300 lines
16 KiB
C
300 lines
16 KiB
C
/**
|
||
* @file pwm.h
|
||
* @brief Compact single-header API for the VP44 PWM solenoid controller.
|
||
*
|
||
* Public API is just two functions:
|
||
* pwm_init() — one-time setup; caches cal/flash/getters into rt
|
||
* pwm_service() — per-cycle update; pulls external inputs via getters,
|
||
* runs the 6-stage control pipeline, writes rt outputs
|
||
*
|
||
* External inputs (sensors, CAN, HW) arrive exclusively through the
|
||
* pwm_input_getters_t vtable. Each getter returns a value in native PWM
|
||
* units — put any unit/scale conversion inside your getter body.
|
||
*
|
||
* Every arithmetic choice (MUL vs MULU, SHRA vs SHR, JGE vs JC) mirrors the
|
||
* MCS-96 assembly; see src/ for per-stage commentary + disassembly address
|
||
* cross-references.
|
||
*/
|
||
#ifndef PWM_H
|
||
#define PWM_H
|
||
|
||
#include <stdint.h>
|
||
#include <stdbool.h>
|
||
#include <string.h>
|
||
|
||
/* ── MCS-96 arithmetic primitives ───────────────────────────────────── */
|
||
#define MUL_S16(a, b) ((int32_t)(int16_t)(a) * (int32_t)(int16_t)(b))
|
||
#define MULU_U16(a, b) ((uint32_t)(uint16_t)(a) * (uint32_t)(uint16_t)(b))
|
||
#define CLAMP(v, lo, hi) ((v) < (lo) ? (lo) : (v) > (hi) ? (hi) : (v))
|
||
#define INTEG_HI(s) ((int16_t)(((s) >> 16) & 0xFFFF))
|
||
#define INTEG_LO(s) ((uint16_t)((s) & 0xFFFF))
|
||
|
||
/** Portable signed right shift — guarantees sign extension (SHRA/SHRAL). */
|
||
static inline int16_t shra16(int16_t v, unsigned n) {
|
||
if (!n) return v;
|
||
uint16_t u = (uint16_t)v, sign = (u >> 15) & 1u;
|
||
uint16_t m = sign ? (uint16_t)(((uint16_t)~0u) << (16u - n)) : 0u;
|
||
return (int16_t)((u >> n) | m);
|
||
}
|
||
static inline int32_t shra32(int32_t v, unsigned n) {
|
||
if (!n) return v;
|
||
uint32_t u = (uint32_t)v, sign = (u >> 31) & 1u;
|
||
uint32_t m = sign ? (~(uint32_t)0u << (32u - n)) : 0u;
|
||
return (int32_t)((u >> n) | m);
|
||
}
|
||
|
||
/* ── Mode flag bit definitions (address 0x028c) ─────────────────────── */
|
||
#define MODE_FIRST_CALL 0x01 /* open→closed transition */
|
||
#define MODE_OUTER_REGION 0x02 /* error outside center region */
|
||
#define MODE_NEG_SAT 0x04 /* lower saturation */
|
||
#define MODE_POS_SAT 0x08 /* upper saturation */
|
||
#define MODE_LARGE_POS 0x10 /* large positive error */
|
||
#define MODE_LARGE_NEG 0x20 /* large negative error */
|
||
#define MODE_PREV_MASK 0xC0 /* shadow of prev cycle bits 4-5 */
|
||
|
||
#define INTERP_MAX_ENTRIES 16
|
||
|
||
/* ══════════════════════════════════════════════════════════════════════
|
||
* 1D INTERPOLATION SLOT
|
||
* Decoded by eval_submap once per input, then consumed by combine_submaps
|
||
* (bilinear, takes two slots) or refine_submap (1D, takes one slot).
|
||
* Four int16s; no cycle-to-cycle state.
|
||
* ══════════════════════════════════════════════════════════════════════ */
|
||
typedef struct pwm_interp_slot {
|
||
int16_t row_stride; /* count * 2 — Y-table row byte-stride (A-axis width) */
|
||
int16_t x_interval; /* x[k-1] - x[k] — denominator (dx between breaks) */
|
||
int16_t x_offset; /* input - x[k] — numerator (distance from lower break) */
|
||
int16_t y_byte_off; /* k * 2 — Y-table byte-offset at break k */
|
||
} pwm_interp_slot_t;
|
||
|
||
/* ══════════════════════════════════════════════════════════════════════
|
||
* EXTERNAL INPUTS
|
||
* Populated each cycle by pwm_service via the getter vtable. All values
|
||
* must be in native PWM units — do any unit/scale conversion inside your
|
||
* getter. Address comments are the MCS-96 RAM location of the original
|
||
* variable in the ROM (for cross-reference with disassembly only).
|
||
* ══════════════════════════════════════════════════════════════════════ */
|
||
typedef struct pwm_inputs {
|
||
int16_t ckp_in; /* 0x02f8 — CKP-derived sensed position */
|
||
uint16_t rpm; /* 0x0040 — engine RPM */
|
||
int16_t angle_dec_cmd; /* 0x0042 — CAN: angle-decrease command (sp2_B submap input) */
|
||
int16_t inj_qty_demand; /* 0x0044 — CAN: injection quantity demand (sp0_B + PI large-neg threshold) */
|
||
int16_t b_fb_kw; /* 0x02b4 — CAN: B_FB_KW plunger feedback demand (setpoint baseline) */
|
||
int16_t state_130; /* 0x0130 — CAN: open/closed-loop discriminant (< 0 forces open-loop) */
|
||
uint16_t supply_voltage; /* 0x0142 — battery / supply voltage (shape_eval submap input) */
|
||
int16_t temperature; /* 0x0146 — temperature (sp1_B submap input) */
|
||
/* NOTE: pwm_B's submap input_var=0x0046 in the ROM. That address is RW46,
|
||
* the register into which finalize_angle_request writes active_request
|
||
* at exit (disasm 0x73f4). It is NOT an external input; our port binds
|
||
* that descriptor's input_ptr directly to rt->active_request. */
|
||
} pwm_inputs_t;
|
||
|
||
/** Getter vtable. Each callback reads one external signal from your system
|
||
* and returns it in native PWM units. `ctx` is opaque to the PWM library
|
||
* and is passed back unchanged so your getters can reach their own state.
|
||
* All callbacks are required (no NULL entries).
|
||
*/
|
||
typedef struct pwm_input_getters {
|
||
int16_t (*ckp_in) (void *ctx);
|
||
uint16_t (*rpm) (void *ctx);
|
||
int16_t (*angle_dec_cmd) (void *ctx);
|
||
int16_t (*inj_qty_demand)(void *ctx);
|
||
int16_t (*b_fb_kw) (void *ctx);
|
||
int16_t (*state_130) (void *ctx);
|
||
uint16_t (*supply_voltage)(void *ctx);
|
||
int16_t (*temperature) (void *ctx);
|
||
void *ctx; /* user data; forwarded to every getter */
|
||
} pwm_input_getters_t;
|
||
|
||
/* Forward decls for the vtables */
|
||
typedef struct pwm_calibration pwm_calibration_t;
|
||
typedef struct pwm_flash pwm_flash_t;
|
||
typedef struct pwm_submap_descr pwm_submap_descr_t;
|
||
|
||
/* ══════════════════════════════════════════════════════════════════════
|
||
* RUNTIME STATE
|
||
* Holds everything that persists cycle-to-cycle plus per-cycle working
|
||
* values. External inputs live in rt->inputs (populated each cycle).
|
||
* ══════════════════════════════════════════════════════════════════════ */
|
||
typedef struct pwm_runtime {
|
||
/* External inputs — refreshed each cycle by pwm_service from getters */
|
||
pwm_inputs_t inputs;
|
||
|
||
/* Async / mixed-direction flags.
|
||
* reset_flag — set by user/ISR on reset edge; cleared by supervisor.
|
||
* system_flags_110 — bit 5 (0x20): external "force open-loop" request;
|
||
* bit 0 (0x01): internal latch set by fast_recovery,
|
||
* cleared by PI when error_persist_counter hits zero.
|
||
*/
|
||
uint8_t reset_flag; /* 0x002e */
|
||
uint8_t system_flags_110; /* 0x0110 */
|
||
|
||
/* ── Setpoint pipeline ───────────────────────────────────────────── */
|
||
int16_t compensation_angle; /* 0x02b6 — sum of 3 submap-pair outputs */
|
||
int16_t pre_offset_target; /* 0x012a — (b_fb_kw + compensation_angle) >> 1 */
|
||
int16_t setpoint_offset; /* 0x0150 — user-supplied setpoint bias */
|
||
int16_t target; /* RW5E — final setpoint (clamped) */
|
||
pwm_interp_slot_t setpoint_slot_x; /* 0x02b8 — per-call eval → combine */
|
||
pwm_interp_slot_t setpoint_slot_y; /* 0x02c0 */
|
||
|
||
/* ── Supervisor ──────────────────────────────────────────────────── */
|
||
int16_t cl_enable_counter; /* 0x0340 */
|
||
int16_t cl_error_delta; /* 0x0342 */
|
||
int16_t max_cl_error; /* 0x02f2 */
|
||
int16_t supervisor_state_17e; /* 0x017e — CL-correction accumulator */
|
||
|
||
/* ── PI controller ───────────────────────────────────────────────── */
|
||
int16_t angle_error; /* 0x0278 */
|
||
int16_t angle_error_shaped; /* 0x0276 */
|
||
uint8_t mode_flags; /* 0x028c */
|
||
int32_t integrator_state; /* 0x0288/028a */
|
||
int16_t integrator_gain; /* 0x0286 */
|
||
int16_t active_request; /* 0x0274 — final angle request */
|
||
int16_t estimated_angle; /* 0x02cc */
|
||
int16_t angle_offset; /* 0x017c */
|
||
int16_t cl_correction_raw; /* 0x0176 */
|
||
int16_t recovery_counter; /* 0x0118 */
|
||
int16_t recovery_count_threshold;/* 0x027a */
|
||
int16_t error_persist_counter; /* 0x027c */
|
||
int16_t first_call_gain; /* 0x02ec */
|
||
int16_t pos_error_normalizer; /* 0x02ee */
|
||
int16_t neg_error_normalizer; /* 0x02f0 */
|
||
int16_t min_rpm_openloop; /* 0x015c */
|
||
|
||
/* PI shaping (populated from CAL by pwm_init) */
|
||
int16_t upper_breakpoint, lower_breakpoint;
|
||
int16_t center_slope, upper_outer_slope, lower_outer_slope;
|
||
int16_t upper_integrator_gain, center_integrator_gain, lower_integrator_gain;
|
||
|
||
/* ── PWM output ──────────────────────────────────────────────────── */
|
||
uint16_t pwm_duty; /* 0x02d2 */
|
||
uint16_t pwm_period; /* 0x0330 */
|
||
uint16_t pwm_on_time, pwm_off_time; /* 0x02ce / 0x02d0 — HW compare regs */
|
||
uint8_t pwm_status_flag; /* 0x00d1 */
|
||
int16_t shape_scale; /* 0x0338 */
|
||
int16_t shape_intermediate; /* 0x0332 */
|
||
int16_t shape_output; /* 0x033a */
|
||
uint16_t pwm_min, pwm_max; /* 0x0158 / 0x015a */
|
||
pwm_interp_slot_t pwm_slot_x; /* 0x0290 — pwm_A eval → combine */
|
||
pwm_interp_slot_t pwm_slot_y; /* 0x0298 — pwm_B eval → combine */
|
||
|
||
/* ── Bindings (set once by pwm_init, consumed by pwm_service) ───── */
|
||
const pwm_calibration_t *bound_cal;
|
||
const pwm_flash_t *bound_flash;
|
||
const pwm_input_getters_t *bound_getters;
|
||
} pwm_runtime_t;
|
||
|
||
/* ── Calibration (TABLE[RWA4]+offset) ───────────────────────────────── */
|
||
struct pwm_calibration {
|
||
int16_t target_minimum; /* CAL+0x11E */
|
||
/* Y-table pointers for each submap pair. Each field originally held a
|
||
* 16-bit ROM address (CAL+0x184/186/188). [0]=RPM×InjQty, [1]=RPM×Temp,
|
||
* [2]=RPM×AngleDec — see src/submap_inputvars.txt */
|
||
const int16_t *y_pair[3]; /* CAL+0x184/186/188 */
|
||
/* PI shaping (CAL+0x00FE…0x0116) */
|
||
int16_t pi_upper_breakpoint, pi_lower_breakpoint;
|
||
int16_t pi_center_slope, pi_upper_outer_slope, pi_lower_outer_slope;
|
||
int16_t pi_upper_integrator_gain, pi_center_integrator_gain, pi_lower_integrator_gain;
|
||
|
||
/* compute_closed_loop_correction tuning */
|
||
int16_t closed_loop_gain_const; /* CAL+0x0156 */
|
||
|
||
/* fast_recovery tuning */
|
||
int16_t error_persist_init_count; /* CAL+0x0108 */
|
||
int16_t recovery_rpm_threshold; /* CAL+0x011A */
|
||
|
||
/* Setpoint offset latched once at init by FUN_8643 (0x867d):
|
||
* RAM[0x0150] = CAL[0x52] - (CAL[0x54] + RAM[0x0430])
|
||
* RAM[0x0430] is observed as always 0 at runtime, so the effective
|
||
* constant is CAL[0x52] - CAL[0x54]. */
|
||
int16_t setpoint_offset; /* CAL+0x0052 - CAL+0x0054 (RAM 0x0150) */
|
||
};
|
||
|
||
/* ── Flash-resident tables ──────────────────────────────────────────── */
|
||
struct pwm_flash {
|
||
int16_t max_error_x[INTERP_MAX_ENTRIES];
|
||
int16_t max_error_y[INTERP_MAX_ENTRIES];
|
||
uint16_t max_error_count;
|
||
int16_t max_error_input_addr;
|
||
int16_t request_upper_limit; /* FLASH:9734 */
|
||
int16_t large_pos_error_thresh; /* FLASH:971a */
|
||
int16_t large_neg_error_thresh; /* FLASH:971c */
|
||
int16_t inj_qty_demand_thresh; /* FLASH:9722 */
|
||
uint16_t rpm_breakpoints[8]; /* FLASH:96fe */
|
||
uint16_t rpm_values[8]; /* FLASH:970e */
|
||
int16_t shape_scale_src; /* FLASH:9710 */
|
||
uint16_t period_max_limit, period_min_limit; /* FLASH:96fa / 96fc */
|
||
const int16_t *pwm_y_table; /* FLASH:9768 — 2D Y-table */
|
||
const int16_t *shape_y_table; /* FLASH:9772 — 1D Y-table */
|
||
};
|
||
|
||
/* ── Submap descriptor (mirrors calibration-block layout) ───────────── */
|
||
struct pwm_submap_descr {
|
||
uint16_t flags; /* +0 */
|
||
const int16_t *input_ptr; /* +2 (bound at runtime) */
|
||
uint16_t count; /* +4 */
|
||
const int16_t *x; /* +6 */
|
||
uint16_t input_addr; /* original address (for input_ptr binding) */
|
||
};
|
||
|
||
/** Bind submap descriptor input_ptr fields to the matching source in rt.
|
||
* Most descriptors point at fields in rt->inputs (external signals), but
|
||
* input_addr 0x0046 resolves to rt->active_request — in the ROM that RAM
|
||
* address is RW46, which finalize_angle_request writes with the PI
|
||
* output at its exit (disasm 0x73f4), so the pwm_B submap sees the
|
||
* latest active_request when pwm_output_compute runs.
|
||
* Recognised input_addr values: 0x0040, 0x0042, 0x0044, 0x0046, 0x0142,
|
||
* 0x0146. Others are bound to NULL. */
|
||
static inline void pwm_bind_submap_inputs(pwm_runtime_t *rt,
|
||
pwm_submap_descr_t *descrs,
|
||
uint16_t n)
|
||
{
|
||
for (uint16_t i = 0; i < n; i++) {
|
||
switch (descrs[i].input_addr) {
|
||
case 0x0040: descrs[i].input_ptr = (const int16_t *)&rt->inputs.rpm; break;
|
||
case 0x0042: descrs[i].input_ptr = &rt->inputs.angle_dec_cmd; break;
|
||
case 0x0044: descrs[i].input_ptr = &rt->inputs.inj_qty_demand; break;
|
||
case 0x0046: descrs[i].input_ptr = &rt->active_request; break;
|
||
case 0x0142: descrs[i].input_ptr = (const int16_t *)&rt->inputs.supply_voltage; break;
|
||
case 0x0146: descrs[i].input_ptr = &rt->inputs.temperature; break;
|
||
default: descrs[i].input_ptr = NULL; break;
|
||
}
|
||
}
|
||
}
|
||
|
||
/* ══════════════════════════════════════════════════════════════════════
|
||
* PUBLIC API
|
||
* ══════════════════════════════════════════════════════════════════════ */
|
||
|
||
/** One-time setup. Zero-inits internal state, applies PI shaping from
|
||
* cal, and caches cal/flash/getters into rt for later use by pwm_service.
|
||
* None of the pointer arguments may be NULL. */
|
||
void pwm_init(pwm_runtime_t *rt,
|
||
const pwm_calibration_t *cal,
|
||
const pwm_flash_t *flash,
|
||
const pwm_input_getters_t *getters);
|
||
|
||
/** Per-cycle update. Calls each getter to populate rt->inputs, then
|
||
* runs the 6-stage control pipeline:
|
||
* 1. max_cl_error lookup
|
||
* 2. setpoint computation
|
||
* 3. supervisor (error + CL-enable + clamping)
|
||
* 4. closed-loop correction + angle estimate
|
||
* 5. PI controller
|
||
* 6. PWM duty + HW register split
|
||
* Outputs are written to rt (pwm_on_time, pwm_off_time, pwm_period, etc).
|
||
*/
|
||
void pwm_service(pwm_runtime_t *rt);
|
||
|
||
/** Utility: 1D descending-X piecewise-linear lookup (helper_from_cal). */
|
||
int16_t pwm_interp_lookup(const int16_t *x, const int16_t *y,
|
||
uint16_t n, int16_t in);
|
||
|
||
/* ── Default & ROM-extracted data ───────────────────────────────────── */
|
||
extern const pwm_calibration_t pwm_cal_default;
|
||
extern const pwm_flash_t pwm_flash_default;
|
||
extern const pwm_calibration_t pwm_cal_rom;
|
||
extern const pwm_flash_t pwm_flash_rom;
|
||
|
||
#endif /* PWM_H */
|