From d95e4e18f1e99bf06fe562b4b869c6e94f09f11b Mon Sep 17 00:00:00 2001 From: LucianoDev Date: Thu, 30 Apr 2026 15:03:57 +0200 Subject: [PATCH] 424026 working with pwm and phi. Rpm filtered for acceleration compensation still missing. Only using phi_ad and applying our accel comp --- .cproject | 8 +- Core/Advance_Control/FBKW.c | 6 +- Core/Advance_Control/cal_tables_rom.c | 172 +-- Core/Advance_Control/cal_tables_rom_424026.c | 152 +++ Core/Advance_Control/pwm.c | 611 ++++----- Core/Advance_Control/pwm.h | 387 +++--- Core/CAN_Libs/can_db.c | 2 +- Core/CAN_Libs/can_manager.c | 7 +- Core/Inc/fuel_map.h | 61 +- Core/Inc/id.h | 21 +- Core/Inc/id_424026.h | 91 ++ Core/Kline_Libs/psg_prop.h | 6 + Core/Phi/compute_temp_phi_comp.c | 142 ++ Core/Phi/compute_temp_phi_comp.h | 51 + Core/Phi/phi.c | 1210 ++++++++++++++++++ Core/Phi/phi.h | 263 ++++ Core/Phi/phi_cal_tables.c | 247 ++++ Core/Src/fuel_map.c | 967 ++------------ Core/Src/injection.c | 6 +- Core/Src/main.c | 8 +- Core/Src/stm32g4xx_it.c | 1 + Core/Src/toothed_wheel.c | 9 +- hpsg5-controller_v2-stm32g4.ioc | 2 +- 23 files changed, 2828 insertions(+), 1602 deletions(-) create mode 100644 Core/Advance_Control/cal_tables_rom_424026.c create mode 100644 Core/Inc/id_424026.h create mode 100644 Core/Phi/compute_temp_phi_comp.c create mode 100644 Core/Phi/compute_temp_phi_comp.h create mode 100644 Core/Phi/phi.c create mode 100644 Core/Phi/phi.h create mode 100644 Core/Phi/phi_cal_tables.c diff --git a/.cproject b/.cproject index bad336f..5a213f1 100644 --- a/.cproject +++ b/.cproject @@ -38,6 +38,7 @@ + @@ -59,6 +60,7 @@ + @@ -85,7 +87,7 @@ - + @@ -125,6 +127,7 @@ + @@ -145,6 +148,7 @@ + @@ -171,7 +175,7 @@ - + diff --git a/Core/Advance_Control/FBKW.c b/Core/Advance_Control/FBKW.c index 88b8a09..a09bf0d 100644 --- a/Core/Advance_Control/FBKW.c +++ b/Core/Advance_Control/FBKW.c @@ -97,10 +97,10 @@ static int16_t fbkw_get_b_fb_kw(void *ctx) { return (int16_t)(B_FB_KW*CF_KW); } -/* state_130 (0x0130): CAN-delivered open/closed-loop discriminant. +/* cl_gate_input (0x0130): CAN-delivered open/closed-loop discriminant. * TODO: wire when the corresponding CAN signal is exposed. Using * FBKW_PID_OPEN as a provisional mapping (negative = open-loop sentinel). */ -static int16_t fbkw_get_state_130(void *ctx) { +static int16_t fbkw_get_cl_gate_input(void *ctx) { (void)ctx; return 0; //return (int16_t)(CKP_PULSE_AVAILABLE ? 0 : -1); @@ -127,7 +127,7 @@ void FBKW_init(void) { fbkw_getters.angle_dec_cmd = fbkw_get_angle_dec_cmd; fbkw_getters.inj_qty_demand = fbkw_get_inj_qty_demand; fbkw_getters.b_fb_kw = fbkw_get_b_fb_kw; - fbkw_getters.state_130 = fbkw_get_state_130; + fbkw_getters.cl_gate_input = fbkw_get_cl_gate_input; fbkw_getters.supply_voltage = fbkw_get_supply_voltage; fbkw_getters.temperature = fbkw_get_temperature; fbkw_getters.ctx = NULL; diff --git a/Core/Advance_Control/cal_tables_rom.c b/Core/Advance_Control/cal_tables_rom.c index 7732253..59758d4 100644 --- a/Core/Advance_Control/cal_tables_rom.c +++ b/Core/Advance_Control/cal_tables_rom.c @@ -1,47 +1,58 @@ /** - * @file cal_tables_rom.c (families/t06211/compact_src) - * @brief ROM-decoded t06211 calibration. + * @file cal_tables_rom.c (families/T06215/compact_src) + * @brief ROM-decoded T06215 calibration. * - * AUTO-GENERATED by tools/extract_calibration.py - * Source ROM: rom_eeprom_dump_0000-9FFF_504012.bin - * Calibration base (RWA4): 0x9BD8 - * Flash anchor: 0x7E18 - * Generated: 2026-04-27 12:40:48 + * Source ROM: 424026.bin (Bosch P/N 167002X9001494060200) + * Calibration base: RWA4 = 0x9BD8 + * Flash anchor: 0x9618 + * Generated: 2026-04-29 (hand-extracted via Ghidra MCP + ROM readback) * - * DO NOT EDIT — regenerate with: - * python tools/extract_calibration.py --family t06211 + * Field name → ROM-address binding lives in pwm_addr_map.h. Boot-derived + * values that previously lived as hard-coded literals in pwm.c's + * runtime_reset are now in `init_*` fields here so each variant carries + * its own values without code edits. + * + * Cal-offset deltas vs t06211: + * - PI body thresholds (0x10E, 0x110, 0x116) — same offsets. + * - target/pi clamps (-512) live at cal+0x128 / cal+0x12A in T06215 + * (vs t06211's cal+0x120 / cal+0x122). + * - pwm_y_table_ptr at cal+0x15C; shape_y_table_ptr at cal+0x166. + * - PWM RPM-window block 0xEE..0x104 — UNCHANGED offsets. + * - setpoint_offset = cal+0x4c - cal+0x4e = 3499 - 4147 = -648. + * - s_recovery RPM gate at cal+0x126 (t06211 used cal+0x11E). */ #include "pwm.h" /* ── Submap x/y arrays ──────────────────────────────────────────────── */ -static const int16_t setpoint_x[6] = { 8389, 5872, 4614, 2726, 1426, 0 }; -static const int16_t setpoint_y[6] = { 1707, 1707, 1707, 939, 427, 427 }; +static const int16_t setpoint_x[6] = { 8389, 5872, 3775, 2726, 1426, 0 }; +static const int16_t setpoint_y[6] = { 1707, 1707, 1195, 768, 427, 427 }; -static const int16_t pwm_A_x[9] = { 25166, 18455, 13841, 8389, 5872, 4614, 2726, 1426, 0 }; -static const int16_t pwm_A_y[9] = { 1707, 1365, 1195, 768, 427, 85, 0, -171, -469 }; +static const int16_t pwm_A_x[9] = { 25166, 18455, 13841, 8389, 5872, 3775, 2726, 1426, 0 }; +static const int16_t pwm_A_y[9] = { 1707, 1365, 1195, 768, 427, 85, 0, -171, -469 }; static const int16_t pwm_B_x[10] = { 1707, 1365, 1195, 768, 427, 85, 0, -171, -469, -512 }; -static const int16_t pwm_B_y[10] = { 819, 737, 492, 0, 41, 49, 82, 102, 205, 205 }; +static const int16_t pwm_B_y[10] = { 819, 737, 492, 0, 41, 49, 82, 102, 205, 205 }; static const int16_t shape_x[4] = { 819, 737, 492, 0 }; -static const int16_t shape_y[4] = { 41, 49, 82, 102 }; +static const int16_t shape_y[4] = { 41, 49, 82, 102 }; -/* ── Y-tables (dereferenced from pwm_y_table_ptr / shape_y_table_ptr) ── */ +/* ── Y-tables ────────────────────────────────────────────────────────── */ static const int16_t shape_y_table_rom[4] = { 41, 49, 82, 102 }; +/* PWM bilinear Y-table at flash 0x9E30 — 10 rows × 9 cols (90 entries). */ static const int16_t pwm_y_table_rom[90] = { - /* row 0 */ 205, 205, 205, 205, 205, 205, 0, 0, 0, - /* row 1 */ 1638, 1392, 1229, 962, 778, 512, 0, 0, 0, - /* row 2 */ 1843, 1577, 1433, 1208, 1003, 758, 0, 0, 0, - /* row 3 */ 2457, 2109, 1986, 1761, 1577, 1229, 287, 0, 0, - /* row 4 */ 2867, 2744, 2641, 2457, 2314, 2129, 1269, 205, 205, - /* row 5 */ 3686, 3481, 3460, 3378, 3276, 3174, 2907, 1024, 737, - /* row 6 */ 3890, 3870, 3849, 3829, 3808, 3788, 3235, 1310, 1024, - /* row 7 */ 3890, 3890, 3890, 3890, 3890, 3890, 3890, 2076, 1638, + /* row 0 */ 205, 205, 205, 205, 205, 205, 205, 205, 0, + /* row 1 */ 1568, 1433, 1355, 1065, 799, 205, 205, 205, 0, + /* row 2 */ 1716, 1679, 1556, 1290, 983, 205, 205, 205, 0, + /* row 3 */ 2219, 2129, 2052, 1925, 1679, 880, 205, 205, 0, + /* row 4 */ 2744, 2580, 2518, 2477, 2355, 1740, 1310, 205, 0, + /* row 5 */ 3210, 3071, 3030, 3194, 3194, 3018, 2867, 737, 205, + /* row 6 */ 3702, 3493, 3403, 3583, 3583, 3493, 3071, 1192, 483, + /* row 7 */ 3890, 3890, 3890, 3890, 3890, 3890, 3890, 2076, 1229, /* row 8 */ 3890, 3890, 3890, 3890, 3890, 3890, 3890, 3890, 3481, - /* row 9 */ 3890, 3890, 3890, 3890, 3890, 3890, 3890, 3890, 3890, + /* row 9 */ 3890, 3890, 3890, 3890, 3890, 3890, 3890, 3890, 4095, }; /* ── Descriptors ────────────────────────────────────────────────────── */ @@ -69,70 +80,81 @@ const int16_t *pwm_submap_y_of(uint16_t idx) { switch (idx) { case PWM_SUBMAP_SETPOINT_INTERP: return setpoint_y; - case PWM_SUBMAP_PWM_A: return pwm_A_y; - case PWM_SUBMAP_PWM_B: return pwm_B_y; - case PWM_SUBMAP_SHAPE_EVAL: return shape_y; - default: return NULL; + case PWM_SUBMAP_PWM_A: return pwm_A_y; + case PWM_SUBMAP_PWM_B: return pwm_B_y; + case PWM_SUBMAP_SHAPE_EVAL: return shape_y; + default: return NULL; } } /* ── Scalars ────────────────────────────────────────────────────────── */ const pwm_calibration_t pwm_cal_rom = { - .large_pos_error_thresh = 128, /* cal+0x10E */ - .large_neg_error_thresh = (int16_t)0xFF00, /* cal+0x110 */ - .pi_low_clamp = (int16_t)0xFE00, /* cal+0x120 */ - .pi_high_clamp = 1707, /* cal+0x124 */ + /* PI controller error-band thresholds */ + .large_pos_error_thresh = 128, /* cal+0x10E */ + .large_neg_error_thresh = (int16_t)0xFF00, /* cal+0x110 = -256 */ + .pi_low_clamp = (int16_t)0xFE00, /* cal+0x128 = -512 */ + .pi_high_clamp = 1707, /* fallback; runtime uses pi_high_clamp_ceiling */ - /* CAN-decoded setpoint (FUN_64c3) cal constants */ - .b_fb_kw_upper_bound = 7680, /* cal+0x004 */ - .b_fb_kw_lower_bound = (int16_t)0xFD00, /* cal+0x006 */ - /* setpoint_offset = cal+0x4c - cal+0x4e = 3499 - 4156 = -657 */ - .setpoint_offset = (int16_t)0xFD6F, - .target_5e_min_clamp = (int16_t)0xFE00, /* cal+0x122 */ - .can_aux_12e_max = 1451, /* cal+0x002 */ - .error_thresh_114 = 100, /* cal+0x114 */ - .pi_thresh_116 = 96, /* cal+0x116 */ - .pi_sat_count_threshold = 800, /* cal+0x112 = 0x0320 */ - .rpm_threshold_11E = 2936, /* cal+0x11E */ - .pi_cl_rpm_floor = 420, /* RAM[0x605c] = 0x01A4 */ + /* CAN-decoded setpoint cal constants */ + .b_fb_kw_upper_bound = 7680, /* cal+0x004 */ + .b_fb_kw_lower_bound = (int16_t)0xFD00, /* cal+0x006 */ + .setpoint_offset = -648, /* cal+0x4c − cal+0x4e */ + .target_5e_min_clamp = (int16_t)0xFE00, /* cal+0x12A */ + .can_aux_12e_max = 1451, /* cal+0x002 */ - .pwm_detail_x0 = (int16_t)0x9C40, /* cal+0x0EE */ - .pwm_detail_x1 = (int16_t)0x8235, /* cal+0x0F0 */ - .pwm_cached_ptr_0F2 = 5662, /* cal+0x0F2 (legacy scalar) */ - .pwm_cached_ptr_102 = 168, /* cal+0x102 (legacy scalar) */ + /* Recovery / sustained-error machinery */ + .pi_state_c2_reload = 100, /* cal+0x114 (ROM 0x9CEC = 0x0064) */ + .inj_qty_thresh = 96, /* cal+0x116 (ROM 0x9CEE = 0x0060) */ + .pi_sat_count_threshold = 800, /* cal+0x112 (ROM 0x9CEA = 0x0320) */ + .rpm_threshold_recovery = 2936, /* cal+0x126 (ROM 0x9CFE = 0x0B78) — was wrong (256, t06211 holdover) */ + .pi_cl_rpm_floor = 420, /* flash[0x605C] = 0x01A4 */ - /* RPM-window matching: inline 8-int16 band array at cal+0x0F2..0x100 - * (4 (lo,hi) pairs) plus halfwidth at cal+0x102. Drives the three- - * phase pwm_period slew in FUN_5314 (open-questions §5 closeout). */ - .pwm_rpm_windows = { 5662, 6795, 8808, 10486, 11954, 13422, 18036, 19713 }, + /* PI runtime-reset values (ROM-extracted from t06215 cal block at + * RWA4=0x9BD8 + offset). Boot multipliers DAT_0410..0416 are + * uninitialized RAM (no writers anywhere in ROM), so they default + * to 0 — the boot equation `(trim_byte << N) + cal_value` resolves + * to cal_value. Earlier defaults of ±853 / 480 / etc. were t06211 + * holdovers from the original fork; corrected 2026-04-29 against + * the actual t06215 ROM bytes. */ + .init_p_shape_bound_pos = +107, /* cal+0x10A (ROM 0x9CE2 = 0x006B) → DAT_0450 */ + .init_p_shape_bound_neg = -107, /* cal+0x10C (ROM 0x9CE4 = 0xFF95) → DAT_0452 */ + .init_p_gain_normal = +336, /* cal+0x118 (ROM 0x9CF0 = 0x0150) → DAT_0454 */ + .init_p_slope_large_pos = +1792, /* cal+0x11A (ROM 0x9CF2 = 0x0700) → DAT_027e */ + .init_p_slope_large_neg = +512, /* cal+0x11C (ROM 0x9CF4 = 0x0200) → DAT_0280 */ + .init_integ_step_normal = +256, /* cal+0x11E (ROM 0x9CF6 = 0x0100) → DAT_0456 */ + .init_integ_step_large_pos = +512, /* cal+0x120 (ROM 0x9CF8 = 0x0200) → DAT_0282 */ + .init_integ_step_large_neg = +256, /* cal+0x122 (ROM 0x9CFA = 0x0100) → DAT_0284 */ + .init_open_loop_p_gain = +6, /* cal+0x124 (ROM 0x9CFC = 0x06, byte, clamped to 15) → DAT_033e */ + + /* CL correction normalizers — boot-cached at DAT_0340/DAT_0342 in + * the ROM (NOT 0x0332/0x0334 — that was a t06211 holdover). Source + * cal offsets are 0x108 (pos) and 0x106 (neg); both equal 350 in + * this ROM. */ + .init_pos_error_normalizer = 350, /* cal+0x108 (ROM 0x9CE0 = 0x015E) → DAT_0340 */ + .init_neg_error_normalizer = 350, /* cal+0x106 (ROM 0x9CDE = 0x015E) → DAT_0342 */ + + /* PWM stage scalars */ + .pwm_detail_x0 = (int16_t)0x9C40, + .pwm_detail_x1 = (int16_t)0x8235, + .pwm_cached_ptr_0F2 = 5662, + .pwm_cached_ptr_102 = 168, + .pwm_const_104 = 354, + + .pwm_rpm_windows = { 5662, 6837, 8808, 10486, 11954, 13422, 18036, 19713 }, .pwm_window_halfwidth = 168, + .pwm_slew_step = 354, - /* pwm_const_104 / pwm_slew_step are the same value (cal+0x104 = 354); - * pwm_const_104 retained for backward compat, pwm_slew_step is the - * semantic name used by the RPM-window matcher (per-cycle pwm_period - * slew magnitude, applied at Phase 1 / Phase 3). Cached to - * RAM[0x02ec] at FUN_5314:0x53b9. */ - .pwm_const_104 = 354, /* cal+0x104 (legacy alias) */ - .pwm_slew_step = 354, /* cal+0x104 (semantic) */ + .pwm_y_table = pwm_y_table_rom, + .shape_y_table = shape_y_table_rom, - .pwm_y_table = pwm_y_table_rom, /* cal+0x154 @ 0x9E28 */ - .shape_y_table = shape_y_table_rom, /* cal+0x15E @ 0x9E20 */ + .closed_loop_gain_const = 10, - /* CL-correction gain — cached at absolute ROM[0x6056], not cal-relative. */ - .closed_loop_gain_const = 10, /* ROM[0x6056] */ + .pwm_period_min = 33333, + .pwm_period_max = 40000, - /* PWM period endpoints (alias of pwm_detail_x0/x1) as unsigned. */ - .pwm_period_min = 33333, /* cal+0x0F0 */ - .pwm_period_max = 40000, /* cal+0x0EE */ - - /* Duty clamp bounds — RAM[0x6058]/RAM[0x605a] mirrors. Same defaults - * as the default family's pwm_min/pwm_max. */ - .pwm_min = 0x00CD, /* RAM[0x6058] = 205 */ - .pwm_max = 0x0F32, /* RAM[0x605a] = 3890 */ + .pwm_min = 0x00CD, + .pwm_max = 0x0F32, }; -/* Family-1 API parity placeholder — t06211 keeps Y-tables inside - * pwm_cal_rom, so pwm_flash_rom has no data. Callers pass &pwm_flash_rom - * to pwm_init() purely for signature compatibility. */ const pwm_flash_t pwm_flash_rom = { 0 }; diff --git a/Core/Advance_Control/cal_tables_rom_424026.c b/Core/Advance_Control/cal_tables_rom_424026.c new file mode 100644 index 0000000..3cb585b --- /dev/null +++ b/Core/Advance_Control/cal_tables_rom_424026.c @@ -0,0 +1,152 @@ +/** + * @file cal_tables_rom.c (families/T06215/compact_src) + * @brief ROM-decoded T06215 calibration. + * + * Source ROM: 424026.bin (Bosch P/N 167002X9001494060200) + * Calibration base: RWA4 = 0x9BD8 + * Flash anchor: 0x9618 + * Generated: 2026-04-28 (hand-extracted via Ghidra MCP + ROM readback) + * + * Mirrors families/t06211/compact_src/cal_tables_rom.c — same struct + * fields and roles; values and Y-tables come from the T06215 ROM. See + * families/T06215/cal_offsets.py for the offset map and + * families/T06215/docs/algorithm-diff-vs-t06211.md for cal-offset deltas + * vs t06211 (most are +8 bytes shifted in the descriptor block). + */ +#include "pwm.h" + +/* ── Submap x/y arrays ──────────────────────────────────────────────── */ + +static const int16_t setpoint_x[6] = { 8389, 5872, 3775, 2726, 1426, 0 }; +static const int16_t setpoint_y[6] = { 1707, 1707, 1195, 768, 427, 427 }; + +static const int16_t pwm_A_x[9] = { 25166, 18455, 13841, 8389, 5872, 3775, 2726, 1426, 0 }; +static const int16_t pwm_A_y[9] = { 1707, 1365, 1195, 768, 427, 85, 0, -171, -469 }; + +static const int16_t pwm_B_x[10] = { 1707, 1365, 1195, 768, 427, 85, 0, -171, -469, -512 }; +static const int16_t pwm_B_y[10] = { 819, 737, 492, 0, 41, 49, 82, 102, 205, 205 }; + +static const int16_t shape_x[4] = { 819, 737, 492, 0 }; +static const int16_t shape_y[4] = { 41, 49, 82, 102 }; + +/* ── Y-tables (dereferenced from pwm_y_table_ptr / shape_y_table_ptr) ── */ + +static const int16_t shape_y_table_rom[4] = { 41, 49, 82, 102 }; + +/* PWM bilinear Y-table at flash 0x9E30 — 10 rows × 9 cols (90 entries). + * Row index = pwm_B output (descending); col index = pwm_A output. */ +static const int16_t pwm_y_table_rom[90] = { + /* row 0 */ 205, 205, 205, 205, 205, 205, 205, 205, 0, + /* row 1 */ 1568, 1433, 1355, 1065, 799, 205, 205, 205, 0, + /* row 2 */ 1716, 1679, 1556, 1290, 983, 205, 205, 205, 0, + /* row 3 */ 2219, 2129, 2052, 1925, 1679, 880, 205, 205, 0, + /* row 4 */ 2744, 2580, 2518, 2477, 2355, 1740, 1310, 205, 0, + /* row 5 */ 3210, 3071, 3030, 3194, 3194, 3018, 2867, 737, 205, + /* row 6 */ 3702, 3493, 3403, 3583, 3583, 3493, 3071, 1192, 483, + /* row 7 */ 3890, 3890, 3890, 3890, 3890, 3890, 3890, 2076, 1229, + /* row 8 */ 3890, 3890, 3890, 3890, 3890, 3890, 3890, 3890, 3481, + /* row 9 */ 3890, 3890, 3890, 3890, 3890, 3890, 3890, 3890, 4095, +}; + +/* ── Descriptors ────────────────────────────────────────────────────── */ + +pwm_submap_descr_t pwm_submap_descrs[PWM_SUBMAP_COUNT] = { + [PWM_SUBMAP_SETPOINT_INTERP] = { + .flags = 0, .input_ptr = NULL, .count = 6, + .x = setpoint_x, .input_addr = 0x0040, /* RPM */ + }, + [PWM_SUBMAP_PWM_A] = { + .flags = 0, .input_ptr = NULL, .count = 9, + .x = pwm_A_x, .input_addr = 0x0040, /* RPM */ + }, + [PWM_SUBMAP_PWM_B] = { + .flags = 0, .input_ptr = NULL, .count = 10, + .x = pwm_B_x, .input_addr = 0x0046, /* active_request feedback */ + }, + [PWM_SUBMAP_SHAPE_EVAL] = { + .flags = 0, .input_ptr = NULL, .count = 4, + .x = shape_x, .input_addr = 0x0142, /* supply_voltage */ + }, +}; + +const int16_t *pwm_submap_y_of(uint16_t idx) +{ + switch (idx) { + case PWM_SUBMAP_SETPOINT_INTERP: return setpoint_y; + case PWM_SUBMAP_PWM_A: return pwm_A_y; + case PWM_SUBMAP_PWM_B: return pwm_B_y; + case PWM_SUBMAP_SHAPE_EVAL: return shape_y; + default: return NULL; + } +} + +/* ── Scalars ────────────────────────────────────────────────────────── */ +/* + * Cal-offset deltas vs t06211 (see algorithm-diff-vs-t06211.md): + * - PI body thresholds (0x10E, 0x110, 0x116) — same offsets. + * - target/pi clamps (-512) live at cal+0x128 / cal+0x12A in T06215 + * (vs t06211's cal+0x120 / cal+0x122). + * - pwm_y_table_ptr at cal+0x15C (vs t06211 0x154). + * - shape_y_table_ptr at cal+0x166 (vs t06211 0x15E). + * - PWM RPM-window block 0xEE..0x104 — UNCHANGED offsets. + * - setpoint_offset = cal+0x4c - cal+0x4e = 3499 - 4147 = -648 + * (vs t06211: 3499 - 4156 = -657). + * + * The pi_high_clamp role in T06215 is filled dynamically by target_344 + * (RPM-driven ceiling) inside pwm.c — but we still populate the field + * here with the setpoint y[] maximum (1707) so any bypass-PI consumer + * sees a sensible upper bound, matching the t06211 convention. + */ + +const pwm_calibration_t pwm_cal_rom = { + /* PI controller thresholds */ + .large_pos_error_thresh = 128, /* cal+0x10E */ + .large_neg_error_thresh = (int16_t)0xFF00, /* cal+0x110 = -256 */ + .pi_low_clamp = (int16_t)0xFE00, /* cal+0x128 = -512 (was cal+0x120 in t06211) */ + .pi_high_clamp = 1707, /* setpoint y[] max — fallback upper clamp */ + + /* CAN-decoded setpoint (FUN_623d, T06215 analog of t06211 FUN_64c3) */ + .b_fb_kw_upper_bound = 7680, /* cal+0x004 */ + .b_fb_kw_lower_bound = (int16_t)0xFD00, /* cal+0x006 = -768 */ + /* setpoint_offset = cal+0x4c - cal+0x4e = 3499 - 4147 = -648 */ + .setpoint_offset = -648, + .target_5e_min_clamp = (int16_t)0xFE00, /* cal+0x12A = -512 (was cal+0x122 in t06211) */ + .can_aux_12e_max = 1451, /* cal+0x002 */ + + /* PI body thresholds and counts (most offsets unchanged from t06211) */ + .error_thresh_114 = 100, /* cal+0x114 (placeholder; not freshly extracted) */ + .pi_thresh_116 = 96, /* cal+0x116 */ + .pi_sat_count_threshold = 800, /* cal+0x112 (placeholder; not freshly extracted) */ + .rpm_threshold_11E = 256, /* cal+0x11E in T06215 = pi_init_d456_base (256) */ + .pi_cl_rpm_floor = 420, /* flash[0x605C] = 0x01A4 (direct flash) */ + + /* PWM stage scalars — offsets cal+0xEE..0x104 unchanged from t06211 */ + .pwm_detail_x0 = (int16_t)0x9C40, /* cal+0x0EE = 40000 */ + .pwm_detail_x1 = (int16_t)0x8235, /* cal+0x0F0 = 33333 */ + .pwm_cached_ptr_0F2 = 5662, /* cal+0x0F2 (legacy scalar) */ + .pwm_cached_ptr_102 = 168, /* cal+0x102 (legacy scalar) */ + + /* RPM-window matching (8 breakpoints, 4 lo/hi pairs) at cal+0xF2..0x100 */ + .pwm_rpm_windows = { 5662, 6837, 8808, 10486, 11954, 13422, 18036, 19713 }, + .pwm_window_halfwidth = 168, /* cal+0x102 */ + + .pwm_const_104 = 354, /* cal+0x104 (legacy alias) */ + .pwm_slew_step = 354, /* cal+0x104 (semantic) */ + + .pwm_y_table = pwm_y_table_rom, /* cal+0x15C @ 0x9E30 */ + .shape_y_table = shape_y_table_rom, /* cal+0x166 @ 0x9E28 */ + + /* CL-correction gain — direct flash, NOT cal-relative */ + .closed_loop_gain_const = 10, /* flash[0x6056] */ + + /* PWM period endpoints (same role + values as t06211 → 50–60 Hz at 2 MHz) */ + .pwm_period_min = 33333, /* cal+0x0F0 */ + .pwm_period_max = 40000, /* cal+0x0EE */ + + /* Duty clamp bounds — direct-flash mirrors at 0x6058/0x605A */ + .pwm_min = 0x00CD, /* flash[0x6058] = 205 */ + .pwm_max = 0x0F32, /* flash[0x605A] = 3890 */ +}; + +/* Family-1 API parity placeholder — Y-tables live inside pwm_cal_rom. */ +const pwm_flash_t pwm_flash_rom = { 0 }; diff --git a/Core/Advance_Control/pwm.c b/Core/Advance_Control/pwm.c index 30472b9..24bf85b 100644 --- a/Core/Advance_Control/pwm.c +++ b/Core/Advance_Control/pwm.c @@ -1,22 +1,24 @@ /** - * @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. + * @file pwm.c (families/T06215/compact_src) + * @brief Compact single-file implementation of the T06215 PWM control + * pipeline. * - * 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 + * 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 * - * Per-block address citations follow the families/t06211/src/ per-module - * ports verbatim; see those files for expanded commentary. + * 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 for file-static helpers. */ +/* 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, @@ -35,47 +37,36 @@ 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 runtime_reset(pwm_runtime_t *rt) -{ - memset(rt, 0, sizeof(*rt)); - /* Normalizers are ROM-resident; fallback defaults for safety. */ - rt->pos_error_normalizer = 500; - rt->neg_error_normalizer = 650; - - /* PI Block-4 error-window bounds — mirror the one-shot boot init - * done by FUN_76aa in the real ROM. Scratch bytes at RAM[0x0414] / - * [0x0416] have no writers anywhere in the ROM (EEPROM trim bytes, - * default 0), so the bounds resolve to cal+0x10A (=+853) and - * cal+0x10C (=-853). See docs/open-questions.md §2. */ - rt->pi_error_bound_pos = +853; - rt->pi_error_bound_neg = -853; - - /* PI compensation/integrator scalars — defaults from FUN_76aa - * boot init. cal+0x118=+480, cal+0x11A=+256, cal+0x11C=6 (byte). */ - rt->pi_post_scale_454 = 480; - rt->pi_integ_step_456 = 256; - rt->pi_integ_gain_330 = 6; - - /* CAN-setpoint defaults. setpoint_offset_150 mirrors family-1's - * pwm_init copy pattern (compact_src/pwm.c:66) — it's a cal-derived - * static bias, not a runtime value. */ - rt->rw42_state = 0; - rt->can_raw_b_fb_kw = 0; - rt->can_aux_12e = 0; - rt->target_5e = 0; - rt->target_336 = 0; -} - static void apply_cal(pwm_runtime_t *rt, const pwm_calibration_t *cal) { - /* Mirrors family-1's pwm_init copy of cal→runtime statics. */ - rt->setpoint_offset_150 = cal->setpoint_offset; + /* 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, @@ -83,7 +74,7 @@ void pwm_init(pwm_runtime_t *rt, const pwm_flash_t *flash, const pwm_input_getters_t *getters) { - (void)flash; /* family-1 API parity; t06211 keeps Y-tables in cal */ + (void)flash; runtime_reset(rt); rt->bound_cal = cal; rt->bound_getters = getters; @@ -97,22 +88,18 @@ static void read_inputs(pwm_runtime_t *rt) 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.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.cl_gate_input = g->cl_gate_input (ctx); rt->inputs.supply_voltage = g->supply_voltage(ctx); - rt->inputs.temperature = g->temperature (ctx); /* accepted; unused */ + rt->inputs.temperature = g->temperature (ctx); - /* 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. + * Interpolation (FUN_7168, fingerprint #3) * ═════════════════════════════════════════════════════════════════════ */ int16_t pwm_interp_lookup(const int16_t *x, const int16_t *y, @@ -131,19 +118,11 @@ int16_t pwm_interp_lookup(const int16_t *x, const int16_t *y, int32_t prod = MUL_S16(num, dy); int16_t quot = (int16_t)(prod / (int32_t)dx); - /* Disasm 0x71c5: ADD RW1C, -0x2[RW20]. After the disasm's pointer - * advance to the y[] half (0x71b2 ADD RW20, RW1E), RW20 points one - * past the breakpoint where the search settled, so -0x2[RW20] is - * y at that breakpoint — which is y[k] in C's k-naming (k = first - * index where in >= x[k] given descending x). Earlier this returned - * y[k-1], producing wrong-polarity setpoints (e.g. target_336 = 1962 - * instead of 1195 at rpm=3355). */ return (int16_t)(quot + y[k]); } /* ═════════════════════════════════════════════════════════════════════ - * Submap eval / bilinear combine / refine — t06211 FUN_6fb8 / FUN_7035 / - * FUN_7014. Scratch layout matches the family-1 ROM convention. + * Submap eval / bilinear combine / refine * ═════════════════════════════════════════════════════════════════════ */ static void s_eval_submap(const pwm_submap_descr_t *d, @@ -165,7 +144,6 @@ static void s_eval_submap(const pwm_submap_descr_t *d, } 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; @@ -221,7 +199,6 @@ static int16_t s_refine(const int16_t *y_base, 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) { @@ -230,7 +207,8 @@ static int16_t s_interp_descr(const pwm_submap_descr_t *d, } /* ═════════════════════════════════════════════════════════════════════ - * Stage 1 — Setpoint (FUN_77b3:77b3-77c7 + FUN_7168) + * 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) @@ -238,33 +216,24 @@ static void s_setpoint(pwm_runtime_t *rt) const pwm_submap_descr_t *d = &pwm_submap_descrs[PWM_SUBMAP_SETPOINT_INTERP]; const int16_t *y = pwm_submap_y_of(PWM_SUBMAP_SETPOINT_INTERP); - rt->target_336 = s_interp_descr(d, y); + 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). + * 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; - /* [0x64ca-0x64d6] Bounds check */ if (raw > cal->b_fb_kw_upper_bound) return; if (raw < cal->b_fb_kw_lower_bound) return; - /* [0x64dd] half = raw >> 1 (SHRA — sign-extending) */ int16_t half = shra16(raw, 1); rt->can_half_12a = half; - /* [0x64e5-0x64ea] result = half + RAM[0x150] + RW42 */ - int16_t result = (int16_t)(half + rt->setpoint_offset_150 + rt->rw42_state); - - /* [0x64ed-0x64f9] Lower clamp */ + 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; } @@ -272,38 +241,57 @@ static void s_setpoint_can_decode(pwm_runtime_t *rt, } /* ═════════════════════════════════════════════════════════════════════ - * Stage 2 — Supervisor (FUN_7beb @ 0x7beb-0x7c41) + * 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) { - /* Reset-flag branch (0x7beb-0x7c0c) */ if (rt->reset_flag == 1u) { - rt->reset_flag = 0u; - rt->cl_enable_counter = 0u; - rt->supervisor_state_17e = 0; - rt->b_fb_kw_baseline = rt->inputs.b_fb_kw; + 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. */ int16_t error = (int16_t)(rt->target_5e - rt->inputs.ckp_in); - error = (int16_t)(error + rt->compensation_angle); + 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->target_336) { - rt->angle_error_raw = rt->target_336; + 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. + * Stage 2a — Closed-loop correction * ═════════════════════════════════════════════════════════════════════ */ static void s_cl_correct(pwm_runtime_t *rt, @@ -311,7 +299,6 @@ static void s_cl_correct(pwm_runtime_t *rt, { int16_t correction; - /* Gate on low byte of cl_enable_counter (0x5f1f-0x5f24). */ if ((rt->cl_enable_counter & 0xFFu) == 0u) { correction = 0; } else { @@ -326,13 +313,13 @@ static void s_cl_correct(pwm_runtime_t *rt, : (int16_t)0; } - rt->cl_correction_raw = correction; - rt->supervisor_state_17e = (int16_t)(rt->supervisor_state_17e + correction); - rt->angle_offset = shra16(rt->supervisor_state_17e, 4u); + 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) + * Stage 2 — Publish CL * ═════════════════════════════════════════════════════════════════════ */ static void s_publish_cl(pwm_runtime_t *rt, @@ -343,201 +330,119 @@ static void s_publish_cl(pwm_runtime_t *rt, } /* ═════════════════════════════════════════════════════════════════════ - * Stage 4 — PI controller (FUN_67c4 @ 0x67c4 + FUN_66a8 + FUN_672b) + * 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. * ═════════════════════════════════════════════════════════════════════ */ -/* 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 +/* 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) { - if (rt->pi_shape_flag != rt->pi_flag_c6) { + 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; } - return; } - /* Counter ≥ threshold → latch+reset. Threshold is boot-cached at - * RAM[0x02c0] from cal+0x112 by FUN_6b7b:0x6bd4. */ - if ((uint16_t)rt->pi_state_118 >= (uint16_t)cal->pi_sat_count_threshold) { - rt->system_flags_110 = (uint8_t)(rt->system_flags_110 | 0x01u); - rt->pi_state_118 = 0; - rt->pi_state_c2 = cal->error_thresh_114; - return; - } - /* Gated increment: rpm > rpm_threshold_11E required. */ - if ((int16_t)rt->inputs.rpm <= cal->rpm_threshold_11E) return; - /* Bits 4 or 5 of system_flags abort. */ - if ((rt->system_flags_110 & 0x30u) != 0u) return; - /* c2 cooldown gate — counter advances only after c2 has decayed to 0. */ - if (rt->pi_state_c2 != 0) return; - rt->pi_state_118 = (int16_t)(rt->pi_state_118 + 1); -} - -/* FUN_7c85 @ 0x7c85-0x7cbf — PI integrator step. Anti-windup gates - * the update direction against pi_flag_c7 (0=in-range, 1=clamped low, - * 2=clamped high). Updates the {pi_b4_state:pi_b6_state} 32-bit pair. - * Disasm: - * 7c85 LD RW1C, err - * 7c8a MUL RL1C, [0x0456] ; signed 32-bit, RL1C = err*step - * 7c90 SHLL RL1C, #0x4 ; RL1C <<= 4 - * 7c93 JGE LAB_7ca1 ; if signed result >= 0 - * 7c95-7c9d if pi_flag_c7==1 RET ; (clamped-low → don't push more negative) - * 7c9f SJMP LAB_7cab ; else update - * LAB_7ca1: if pi_flag_c7==2 RET ; (clamped-high → don't push more positive) - * LAB_7cab: integrator += RL1C ; ADD lo, ADDC hi - */ -static void s_pi_integrator_step(pwm_runtime_t *rt) -{ - int32_t prod = MUL_S16(rt->angle_error_pi, rt->pi_integ_step_456); - int32_t step = (int32_t)((uint32_t)prod << 4); - - if (step >= 0) { - if (rt->pi_flag_c7 == 0x02) return; /* anti-windup: clamped-high */ - } else { - if (rt->pi_flag_c7 == 0x01) return; /* anti-windup: clamped-low */ - } - - /* Combine {hi:lo} as int32 -> add step -> split back. */ - int32_t accum = ((int32_t)(int16_t)rt->pi_b4_state << 16) - | (uint16_t)rt->pi_b6_state; - accum += step; - rt->pi_b6_state = (int16_t)(accum & 0xFFFF); - rt->pi_b4_state = (int16_t)((accum >> 16) & 0xFFFF); -} - -/* FUN_672b @ 0x672b-0x67c3 — PI compensation + final clamp. - * Two arms decided by (pi_flag_338==1 || pi_open_loop_flag==0): - * "fresh" → reset pi_b4_state from a new (err*pi_gain>>4)+ckp formula - * "settled" → step the integrator via FUN_7c85 - * Then compute compensation_angle = (err * pi_post_scale_454) >> 8 - * and active_request = comp + pi_b4_state, clamped to [pi_low_clamp, target]. - */ -static void s_pi_compensation(pwm_runtime_t *rt, - const pwm_calibration_t *cal) -{ - int16_t error = rt->angle_error_pi; - - /* [0x672b-0x673a] Branch select. */ - bool fresh = (rt->pi_flag_338 == 0x01) || (rt->pi_open_loop_flag == 0u); - - if (fresh) { - /* [LAB_673c 0x673c-0x6762] Reset pi_b4_state. Disasm: - * STB ZRlo, DAT_0338 ; pi_flag_338 = 0 - * LD RW1C, err - * EXT RL1C ; sign-extend to 32-bit - * MUL RL1C, [0x0330] ; signed: RL1C = err32 * pi_gain - * SHRA RW1C, #0x4 ; LOW WORD shifted (high word discarded) - * ADD RW1C, RW1C, ckp_in - * ST RW1C, DAT_02b4 ; pi_b4_state = result - */ - rt->pi_flag_338 = 0; - int32_t prod = MUL_S16(error, (int16_t)rt->pi_integ_gain_330); - int16_t scaled = shra16((int16_t)(prod & 0xFFFF), 4); - rt->pi_b4_state = (int16_t)(scaled + rt->inputs.ckp_in); - } else { - /* [LAB_6764 0x6764] LCALL FUN_7c85 — integrator step. */ - s_pi_integrator_step(rt); - } - - /* [LAB_6767 0x6767-0x6779] compensation_angle = (RL1C = post_scale*err) >> 8. - * Disasm: - * LD RW1C, [0x0454] ; post_scale (default +480) - * MUL RL1C, err ; RL1C = post_scale * err (signed) - * SHRAL RL1C, #0x8 ; arithmetic shift right 8 (32-bit) - * ST RW1C, DAT_02b8 ; compensation_angle = lo16 - */ - int32_t comp32 = MUL_S16(rt->pi_post_scale_454, error); - int16_t comp = (int16_t)(shra32(comp32, 8) & 0xFFFF); - rt->compensation_angle = comp; - - /* [0x677a-0x677f] active_request = comp + pi_b4_state */ - int16_t combined = (int16_t)(comp + rt->pi_b4_state); - rt->active_request = combined; - - /* [0x6782-0x67a8] Lower clamp: if active_request < pi_low_clamp, - * pin to pi_low_clamp and set pi_flag_c7=1. */ - if (rt->active_request < cal->pi_low_clamp) { - rt->active_request = cal->pi_low_clamp; - rt->pi_flag_c7 = 0x01; - return; - } - - /* [LAB_67a9 0x67a9-0x67bd] Upper clamp: if active_request > target, - * pin to target and set pi_flag_c7=2. */ - /* Disasm 0x67a9-0x67b0: CMP RW46, DAT_0336 — upper clamp uses - * the RPM-derived ceiling, not the CAN-decoded primary setpoint. */ - if (rt->active_request > rt->target_336) { - rt->active_request = rt->target_336; - rt->pi_flag_c7 = 0x02; - return; - } - - /* [LAB_67be 0x67be-0x67c3] In-range: pi_flag_c7 = 0. */ - rt->pi_flag_c7 = 0; } +/* 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) { - /* PI CL-gate threshold = cached ROM[0x605c]. Disasm site: - * FUN_67c4:67d9 `CMP RW1C, DAT_605c` (literal 0x01A4 = 420). In the - * real ECU, state_130 is almost always negative (CAN inactive - * sentinel) which forces OL at line 67d4 before this RPM gate is - * reached. Lifted to cal->pi_cl_rpm_floor. */ - int16_t rpm_floor = cal->pi_cl_rpm_floor; + /* ── 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.state_130 < 0) open_loop = true; - else if ((int16_t)rt->inputs.rpm < rpm_floor) open_loop = true; + 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) { - 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; + /* 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 final_flag; + goto trailing_rotate; } - /* 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; + /* ── 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) { - rt->pi_shape_flag = 0; + /* 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) { - rt->pi_shape_flag = 0x01; - if (rt->inputs.inj_qty_demand <= cal->pi_thresh_116) { - rt->pi_state_118 = 0; - } else { + /* 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 { - rt->pi_shape_flag = 0x20; - if (rt->pi_state_c2 > 0) { + /* 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) { @@ -548,73 +453,125 @@ static void s_pi_update(pwm_runtime_t *rt, } } - /* ── Block 4: error-window decision [0x68b7-0x68ef] ── - * Strict > / < per disasm JLE 68c9 / JGE 68ed. Bounds at - * 0x0450/0x0452 are set once at boot by FUN_76aa — see - * docs/open-questions.md §2 closeout. */ - if (err > rt->pi_error_bound_pos) { - rt->active_request = cal->pi_high_clamp; - rt->pi_flag_338 = 0x01; - } else if (err < rt->pi_error_bound_neg) { - rt->active_request = cal->pi_low_clamp; - rt->pi_flag_338 = 0x01; + /* ── 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 { - s_pi_compensation(rt, cal); + /* 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); } -final_flag: - rt->pi_open_loop_flag = 0x01; - rt->pi_flag_c6 = rt->pi_shape_flag; + /* ── 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 5 — PWM output (FUN_5314 @ 0x5314-0x565f) + * Stage 4 — 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]; @@ -627,38 +584,31 @@ static void s_pwm_output(pwm_runtime_t *rt, } 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 */ + continue; } if (rpm_s < (int16_t)(hi + halfwidth)) { - phase3 = 0; /* HOLD */ + phase3 = 0; 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); @@ -667,11 +617,6 @@ static void s_pwm_output(pwm_runtime_t *rt, 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 @@ -681,14 +626,10 @@ static void s_pwm_output(pwm_runtime_t *rt, : 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); @@ -696,10 +637,7 @@ static void s_pwm_output(pwm_runtime_t *rt, } /* ═════════════════════════════════════════════════════════════════════ - * 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. + * Bypass-PI LUT helper * ═════════════════════════════════════════════════════════════════════ */ uint16_t pwm_lut_duty(const pwm_calibration_t *cal, @@ -720,8 +658,7 @@ uint16_t pwm_lut_duty(const pwm_calibration_t *cal, } /* ═════════════════════════════════════════════════════════════════════ - * Public entry — pwm_service - * Mirrors FUN_77b3 (0x77b3-0x77d8) — 5-call linear dispatcher. + * Public entry — pwm_service (mirrors pwm_service @ 0x7780) * ═════════════════════════════════════════════════════════════════════ */ void pwm_service(pwm_runtime_t *rt) @@ -730,10 +667,10 @@ void pwm_service(pwm_runtime_t *rt) const pwm_calibration_t *cal = rt->bound_cal; - s_setpoint(rt); /* [0x77b3-0x77c7] target_336 = pwm_A interp(rpm) */ - s_setpoint_can_decode(rt, cal); /* CAN-decoded target (FUN_64c3 in ROM) */ - s_supervisor(rt); /* [0x77cc] */ - s_publish_cl(rt, cal);/* [0x77cf] */ - s_pi_update(rt, cal); /* [0x77d2] */ - s_pwm_output(rt, cal);/* [0x77d5] */ + 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); } diff --git a/Core/Advance_Control/pwm.h b/Core/Advance_Control/pwm.h index 6981cb8..d5b1c3b 100644 --- a/Core/Advance_Control/pwm.h +++ b/Core/Advance_Control/pwm.h @@ -1,21 +1,20 @@ /** - * @file pwm.h (families/t06211/compact_src) - * @brief Compact single-header API for the t06211 PWM controller. + * @file pwm.h (families/T06215/compact_src) + * @brief Compact single-header API for the T06215 PWM controller. * - * Mirrors the public shape of the default family's `compact_src/pwm.h` - * but tailored to t06211's 5-stage pipeline (no standalone max_cl_error - * lookup, single-submap setpoint, factor+offset target compute). + * Variant note: this header was originally forked from t06211 and carried + * runtime/cal field names that embedded RAM addresses (e.g. `pi_b4_state`, + * `target_336`). Those addresses were t06211-correct but t06215-wrong. + * Field names here are now purely semantic; the address binding for any + * field lives in `pwm_addr_map.h` (documentation-only; not included by + * pwm.c). * - * Public API is just two functions: + * Public API: * pwm_init() — one-time setup - * pwm_service() — per-cycle update (pulls inputs via getters, - * runs the 5-stage pipeline, writes rt outputs) + * pwm_service() — per-cycle update (5-stage pipeline) * - * External inputs arrive through a getter vtable; each callback returns - * one signal in native PWM units. - * - * Every arithmetic choice mirrors the MCS-96 assembly; see - * families/t06211/src/ for per-stage commentary. + * External inputs arrive through a getter vtable; callbacks return one + * signal in native PWM units. */ #ifndef PWM_H #define PWM_H @@ -44,49 +43,37 @@ static inline int32_t shra32(int32_t v, unsigned n) { /* ══════════════════════════════════════════════════════════════════════ * 1D INTERPOLATION SLOT - * Layout matches family-1's pwm_interp_slot_t and the family-1 ROM - * convention [count*2, range, offset, seg_bytes] — t06211's FUN_6fb8 - * fingerprints identically, so the same layout applies. * ══════════════════════════════════════════════════════════════════════ */ typedef struct pwm_interp_slot { - int16_t row_stride; /* count * 2 (byte stride for Y-table rows) */ - int16_t x_interval; /* x[k-1] - x[k] */ - int16_t x_offset; /* input - x[k] */ - int16_t y_byte_off; /* k * 2 */ + int16_t row_stride; + int16_t x_interval; + int16_t x_offset; + int16_t y_byte_off; } pwm_interp_slot_t; /* ══════════════════════════════════════════════════════════════════════ * EXTERNAL INPUTS - * Populated each cycle by pwm_service via the getter vtable. - * t06211-specific set: no temperature, no angle_dec_cmd in the setpoint - * (the family-2 setpoint is single-submap RPM-indexed). * ══════════════════════════════════════════════════════════════════════ */ typedef struct pwm_inputs { - int16_t ckp_in; /* 0x02f8 — sensed position */ - uint16_t rpm; /* 0x0040 — engine RPM */ - int16_t angle_dec_cmd; /* 0x0042 — accepted for family-1 API parity; unused here */ - int16_t inj_qty_demand; /* 0x0044 — CAN: inj quantity demand (PI large-neg gate) */ - int16_t b_fb_kw; /* CAN: plunger feedback baseline. Gets copied into - can_raw_b_fb_kw and decoded by s_setpoint_can_decode - (port of FUN_64c3) to produce target. */ - int16_t state_130; /* 0x0130 — CAN: open/closed-loop discriminant */ - uint16_t supply_voltage; /* 0x0142 — shape_eval submap input */ - int16_t temperature; /* 0x0146 — accepted for family-1 API parity; unused here */ + int16_t ckp_in; /* sensed position */ + uint16_t rpm; /* engine RPM */ + int16_t angle_dec_cmd; /* accepted for cross-family API parity; unused here */ + int16_t inj_qty_demand; /* CAN: injection-quantity demand */ + int16_t b_fb_kw; /* CAN: plunger feedback baseline (drives target) */ + int16_t cl_gate_input; /* CAN: open/closed-loop discriminant (was state_130) */ + uint16_t supply_voltage; /* shape_eval submap input */ + int16_t temperature; /* accepted for cross-family API parity; unused here */ } pwm_inputs_t; -/** Getter vtable. 8-callback layout matches family-1's pwm_input_getters_t - * so a single FBKW.c can drive either family's pwm.c without changes. - * t06211 does not consume angle_dec_cmd or temperature; those getters are - * called and stored into rt->inputs but the pipeline ignores them. */ typedef struct pwm_input_getters { int16_t (*ckp_in) (void *ctx); uint16_t (*rpm) (void *ctx); - int16_t (*angle_dec_cmd) (void *ctx); /* accepted; unused by t06211 */ + 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); + int16_t (*cl_gate_input) (void *ctx); uint16_t (*supply_voltage)(void *ctx); - int16_t (*temperature) (void *ctx); /* accepted; unused by t06211 */ + int16_t (*temperature) (void *ctx); void *ctx; } pwm_input_getters_t; @@ -95,128 +82,95 @@ typedef struct pwm_submap_descr pwm_submap_descr_t; /* ══════════════════════════════════════════════════════════════════════ * RUNTIME STATE - * RAM-address comments reference the t06211 MCS-96 map (see - * families/t06211/docs/variable-glossary.md). + * Field names are purely semantic. ROM-address bindings live in + * pwm_addr_map.h (documentation-only). * ══════════════════════════════════════════════════════════════════════ */ typedef struct pwm_runtime { /* External inputs — refreshed each cycle from getters */ pwm_inputs_t inputs; - /* Async flags. - * reset_flag — set on reset edge; supervisor clears. - * system_flags_110 — bit 5 forces open-loop; bit 0 is fast-recovery latch. - */ - uint8_t reset_flag; /* 0x002e */ - uint8_t system_flags_110; /* 0x0110 */ + /* Async flags. */ + uint8_t reset_flag; + uint8_t system_flags_110; /* bit 5 = OL force; bit 0 = recovery latch */ - /* ── Dual-target setpoint architecture ── */ - /* target (RW5E = RAM[0x005e]): PRIMARY CAN-driven setpoint - * computed by FUN_64c3 (called from CAN parser FUN_6192). Drives - * the PI controller in both open-loop (FUN_67c4:67e5) and - * closed-loop (FUN_67c4:680a) paths, and supervisor error compute - * (FUN_7beb:7c20). Formula: raw/2 + setpoint_offset_150 + rw42_state, - * clamped >= cal->target_5e_min_clamp. */ - int16_t target_5e; /* 0x005e — primary CAN-decoded setpoint */ - /* target_336 (DAT_0336): SECONDARY RPM-derived ceiling computed by - * FUN_7168(setpoint_descr) + FUN_77b3:77c7. Used ONLY as upper-clamp - * in FUN_672b PI compensation (0x67a9/0x67b0) and FUN_7beb supervisor - * (0x7c30/0x7c37). Does not drive control directly. */ - int16_t target_336; /* 0x0336 — RPM-derived ceiling */ - /* CAN-staging buffers — written externally before pwm_service so - * s_setpoint_can_decode can transform them into target. */ - int16_t can_raw_b_fb_kw; /* 0x012c — raw b_fb_kw from CAN */ - int16_t can_aux_12e; /* 0x012e — secondary CAN field (drives rw42_state) */ - int16_t can_half_12a; /* 0x012a — cached raw>>1 (debug visibility) */ - /* setpoint_offset_150 (RAM[0x0150]) is copied at runtime_init from - * cal.setpoint_offset (mirrors family-1's pattern at - * compact_src/pwm.c:66). The REAL ROM derives it at boot via - * FUN_6b7b @ 0x6bb5: RAM[0x150] = cal+0x4c - RW1E. Because RW1E's - * source at the call site (0x6c46) is in a Ghidra-unrecognised code - * region, we expose the field directly through cal so the user can - * set it to the match-fitted value without needing to simulate the - * full FUN_6b7b boot path. */ - int16_t setpoint_offset_150; /* 0x0150 */ - int16_t rw42_state; /* RW42 register — = can_aux_12e when valid */ - int16_t b_fb_kw_baseline; /* 0x033a — latched snapshot of b_fb_kw on reset */ - int16_t compensation_angle; /* 0x02b8 — PI-helper output consumed in supervisor */ - int16_t angle_error_raw; /* 0x033e — supervisor output (raw + ceiling-clamped) */ - uint16_t cl_enable_counter; /* 0x033c */ + /* ── Setpoint architecture ── */ + /* target_5e: PRIMARY CAN-decoded setpoint (RW5E in t06215). */ + int16_t target_5e; + /* pi_high_clamp_ceiling: SECONDARY RPM-derived ceiling — written each + * cycle by pwm_service from the setpoint submap interp; consumed as + * the PI integrator output upper-clamp inside s_pi_update and the + * supervisor error ceiling. (DAT_0344 in t06215; was misnamed + * target_336 in the original fork from t06211.) */ + int16_t pi_high_clamp_ceiling; - /* ── CL correction (RAM-layout identical to family 1) ── */ - int16_t cl_correction_raw; /* 0x0176 */ - int16_t angle_offset; /* 0x017c */ - int16_t supervisor_state_17e; /* 0x017e — CL accumulator */ - int16_t pos_error_normalizer; /* 0x0332 */ - int16_t neg_error_normalizer; /* 0x0334 */ + /* CAN-staging buffers — written before pwm_service so s_setpoint_can_decode + * can transform them into target_5e. */ + int16_t can_raw_b_fb_kw; + int16_t can_aux_12e; + int16_t can_half_12a; + int16_t setpoint_offset; /* runtime copy of cal->setpoint_offset */ + int16_t rw42_state; + int16_t angle_error_raw; /* DAT_02f0 — supervisor's published error */ + int16_t pi_integ_hi_snapshot; /* DAT_02ec — supervisor reset stash; dead store but kept for parity */ + uint16_t cl_enable_counter; /* DAT_02ee */ + + /* ── CL correction ── */ + int16_t cl_correction_raw; + int16_t angle_offset; + int16_t supervisor_state; + int16_t pos_error_normalizer; /* runtime copy of cal->pos_error_normalizer */ + int16_t neg_error_normalizer; /* runtime copy of cal->neg_error_normalizer */ /* ── Publish + PI ── */ - int16_t estimated_angle; /* 0x02cc */ - int16_t angle_error_pi; /* 0x02be */ - int16_t active_request; /* 0x0046 (RW46) — PI output / PWM feed-forward */ - uint8_t pi_open_loop_flag; /* 0x02c4 */ - uint8_t pi_shape_flag; /* 0x02c5 */ - uint8_t pi_flag_c6; /* 0x02c6 */ - uint8_t pi_flag_338; /* 0x0338 */ - /* PI Block-4 error-window bounds (independent int16s, NOT a 32-bit - * integrator). Boot-initialised once by FUN_76aa @ 0x76aa — see - * docs/open-questions.md §2 (resolved). Trim bytes at 0x0414/0x0416 - * have no writers in the ROM, so bounds default to ±853 at runtime. */ - int16_t pi_error_bound_pos; /* 0x0450 — default +853 */ - int16_t pi_error_bound_neg; /* 0x0452 — default -853 */ - int16_t pi_state_118; /* 0x0118 */ - int16_t pi_state_c2; /* 0x02c2 */ + int16_t estimated_angle; + int16_t angle_error_pi; /* target_5e − estimated_angle (DAT_0278) */ + int16_t active_request; /* PI output = pre-clamp output (RW46 + DAT_0274) */ - /* PI integrator at {RAM[0x02b4]:RAM[0x02b6]} — was misnamed "b_fb_kw" - * across the original glossary. Disasm `disasm_nav rw 0x02b4` shows - * only internal writers (FUN_672b, FUN_76aa, FUN_7c85), confirming - * this is purely a PI integrator state, not an external/CAN input. - * Treated as a signed 32-bit pair: hi at 0x02b4 (the value used in - * active_request = comp + pi_b4_state), lo at 0x02b6. */ - int16_t pi_b4_state; /* 0x02b4 — integrator high word */ - int16_t pi_b6_state; /* 0x02b6 — integrator low word */ + /* PI flag byte (DAT_028c). Bit layout (s_pi_update @ 0x542f): + * bit 0 = open-loop indicator (consumed by integrator gate) + * bit 1 = P-shape large-error arm taken + * bit 2 = output clamped to pi_low_clamp this cycle + * bit 3 = output clamped to pi_high_clamp_ceiling this cycle + * bit 4 = error > large_pos_error_thresh this cycle + * bit 5 = error < large_neg_error_thresh this cycle + * bits 6,7 = previous cycle's bits 4,5 (rotated by trailing op) */ + uint8_t pi_shape_flag; - /* PI compensation scalars — boot-set by FUN_76aa from cal+0x118/0x11A - * with optional <<4 byte trims at RAM[0x0410]/[0x0412] (no writers, - * default 0). Defaults: 0x0454=+480 (post-scale), 0x0456=+256 (step). */ - int16_t pi_post_scale_454; /* 0x0454 — multiplier in comp = (err*scale)>>8 */ - int16_t pi_integ_step_456; /* 0x0456 — multiplier in FUN_7c85 integrator step */ + /* PI working state used by s_pi_update */ + int16_t pi_p_term; /* DAT_0276 — output of P-shape segment */ + int16_t pi_p_gain_active; /* DAT_0286 — selected gain for integrator step */ + int16_t pi_preclamp_out; /* DAT_0274 — pre-publish active_request */ - /* PI integ gain — boot-set by FUN_76aa from cal+0x11C (byte, clamped - * ≤15). Used in the "reset" branch as: pi_b4_state = (err*gain)>>4 + ckp. - * The runtime field formerly named "pwm_period" was a holdover from - * family 1 where 0x0330 had a different semantic; in t06211, 0x0330 - * is this PI gain. */ - uint8_t pi_integ_gain_330; /* 0x0330 — boot default 6 (cal+0x11C) */ + /* PI integrator pair {hi:lo} = {DAT_028a:DAT_0288} */ + int16_t pi_integ_lo; + int16_t pi_integ_hi; - /* Anti-windup flag set by FUN_672b clamps; consulted by FUN_7c85 - * to gate integration direction. Values: 0 (in-range), 1 (clamped - * low), 2 (clamped high). */ - uint8_t pi_flag_c7; /* 0x02c7 */ + /* P-shape segment bounds (boot-init from cal+0x10A/0x10C). */ + int16_t p_shape_bound_pos; + int16_t p_shape_bound_neg; + + /* P-shape gains and integrator steps (boot-init from cal). */ + int16_t p_gain_normal; /* DAT_0454 — normal-range P-term multiplier */ + int16_t integ_step_normal; /* DAT_0456 — normal-range integrator multiplier */ + int16_t p_slope_large_pos; /* DAT_027e */ + int16_t p_slope_large_neg; /* DAT_0280 */ + int16_t integ_step_large_pos; /* DAT_0282 */ + int16_t integ_step_large_neg; /* DAT_0284 */ + int16_t open_loop_p_gain; /* DAT_033e (byte, boot-clamped to 15) */ + + int16_t pi_state_118; /* recovery counter */ + int16_t pi_state_c2; /* cooldown counter */ /* ── PWM output ── */ - uint16_t pwm_duty; /* 0x02d2 */ - uint16_t pwm_on_time; /* 0x02ce */ - uint16_t pwm_off_time; /* 0x02d0 */ - /* pwm_period — total period in Timer2 ticks = on_time + off_time. - * Mirrored from t06211 RAM[0x02e4] (computed each cycle by the PWM - * output stage). Exposed for family-1 FBKW.c API parity - * (UpdatePWM(&htim4, ch, pwm_on_time, pwm_period)). */ + uint16_t pwm_duty; + uint16_t pwm_on_time; + uint16_t pwm_off_time; uint16_t pwm_period; - uint8_t pwm_duty_range_flag; /* 0x00d1 */ - pwm_interp_slot_t pwm_slot_a; /* 0x02d4 */ - pwm_interp_slot_t pwm_slot_b; /* 0x02dc */ - /* pwm_shape_state[6] mirrors RAM[0x02e4..0x02ee]: - * [0] pwm_period working value (RAM 0x02e4) - * [1] (unused — was slew_increment, broken out as field below) - * [2] (RAM 0x02e8 — ROM band-array ptr, unused in C) - * [3] (RAM 0x02ea — ROM halfwidth ptr, unused in C) - * [4] (RAM 0x02ec — slew step magnitude, unused; read from cal) - * [5] shape_height (RAM 0x02ee, E2 output) */ - int16_t pwm_shape_state[6]; /* 0x02e4-0x02ee */ - /* Persistent slew_increment (RAM[0x02e6]). Carried across cycles so - * the hysteresis-margin HOLD path can preserve previous direction. - * Subtracted from pwm_period each cycle. See open-questions §5. */ - int16_t pwm_slew_increment; /* 0x02e6 */ + uint8_t pwm_duty_range_flag; + pwm_interp_slot_t pwm_slot_a; + pwm_interp_slot_t pwm_slot_b; + int16_t pwm_shape_state[6]; + int16_t pwm_slew_increment; /* ── Bindings (set once by pwm_init) ── */ const pwm_calibration_t *bound_cal; @@ -225,80 +179,73 @@ typedef struct pwm_runtime { /* ── Calibration (decoded ROM values) ───────────────────────────────── */ struct pwm_calibration { - /* Scalars (from families/t06211/cal_offsets.py FLASH_OFFSETS) */ + /* PI controller error-band thresholds */ int16_t large_pos_error_thresh; /* cal+0x10E */ int16_t large_neg_error_thresh; /* cal+0x110 */ - int16_t pi_low_clamp; /* cal+0x120 = -512 — Block-4 low clamp + open-loop lower bound */ - int16_t pi_high_clamp; /* cal+0x124 = +1707 — Block-4 high clamp */ + int16_t pi_low_clamp; /* cal+0x128 — output low clamp + open-loop lower bound */ + int16_t pi_high_clamp; /* fallback upper clamp; runtime uses pi_high_clamp_ceiling */ - /* CAN-decoded setpoint (FUN_64c3) cal constants */ - int16_t b_fb_kw_upper_bound; /* cal+0x004 = +7680 — raw b_fb_kw upper sanity bound */ - int16_t b_fb_kw_lower_bound; /* cal+0x006 = -768 — raw b_fb_kw lower sanity bound */ + /* CAN-decoded setpoint (FUN_64c3 family) cal constants */ + int16_t b_fb_kw_upper_bound; /* cal+0x004 */ + int16_t b_fb_kw_lower_bound; /* cal+0x006 */ + int16_t setpoint_offset; /* = cal+0x4c − cal+0x4e */ + int16_t target_5e_min_clamp; /* cal+0x12A */ + int16_t can_aux_12e_max; /* cal+0x002 */ - /* setpoint_offset — static bias added after halving raw b_fb_kw. - * Family-1 analog: CAL+0x0052 - CAL+0x0054 (compact_src/pwm.h:210). - * For t06211, FUN_6b7b computes the equivalent as cal+0x4c - cal+0x4e - * (= 3499 - 4156 = -657) at boot and stores at RAM[0x150]. - * Copied into runtime.setpoint_offset_150 by runtime_reset. */ - int16_t setpoint_offset; /* = cal+0x4c - cal+0x4e */ - int16_t target_5e_min_clamp; /* cal+0x122 = -512 — RW5E lower clamp */ - int16_t can_aux_12e_max; /* cal+0x002 — upper bound for RAM[0x12e] (FUN_649e) */ - int16_t error_thresh_114; /* cal+0x114 */ - int16_t pi_thresh_116; /* cal+0x116 */ - /* pi_state_118 saturation threshold — when the recovery counter - * reaches this, FUN_66a8 latches bit0 of system_flags_110, zeroes - * the counter, and reloads pi_state_c2 from error_thresh_114. - * Boot-cached at RAM[0x02c0] by FUN_6b7b:0x6bd4. ROM literal 0x0320 = 800. */ - int16_t pi_sat_count_threshold; /* cal+0x112 */ - int16_t rpm_threshold_11E; /* cal+0x11E — RPM gate inside FUN_66a8 */ - /* Closed-loop entry RPM floor (FUN_67c4:0x67d9). Sourced from - * RAM[0x605c] (flash mirror; ROM literal 0x01A4 = 420). */ - int16_t pi_cl_rpm_floor; + /* Recovery / sustained-error machinery */ + int16_t pi_state_c2_reload; /* cal+0x114 — reload value for pi_state_c2 on latch */ + int16_t inj_qty_thresh; /* cal+0x116 — inj-qty threshold for recovery vs reset */ + int16_t pi_sat_count_threshold; /* cal+0x112 — recovery counter latch threshold */ + int16_t rpm_threshold_recovery; /* cal+0x126 — s_recovery RPM gate (was wrongly cal+0x11E in t06211 idiom) */ + int16_t pi_cl_rpm_floor; /* flash[0x605C] — s_pi_update OL gate threshold */ - int16_t pwm_detail_x0; /* cal+0x0EE */ - int16_t pwm_detail_x1; /* cal+0x0F0 */ - int16_t pwm_cached_ptr_0F2; /* cal+0x0F2 — first RPM-window breakpoint (legacy scalar) */ - int16_t pwm_cached_ptr_102; /* cal+0x102 — window halfwidth (legacy scalar) */ - int16_t pwm_const_104; /* cal+0x104 */ + /* PI runtime-reset values (boot-derived in ROM; cal-resident here so + * each variant carries its own values without code edits). */ + int16_t init_p_shape_bound_pos; /* cal+0x10A — copied to rt->p_shape_bound_pos */ + int16_t init_p_shape_bound_neg; /* cal+0x10C — copied to rt->p_shape_bound_neg */ + int16_t init_p_gain_normal; /* cal+0x118 — copied to rt->p_gain_normal */ + int16_t init_integ_step_normal; /* cal+0x11E — copied to rt->integ_step_normal */ + int16_t init_p_slope_large_pos; /* cal+0x11A — copied to rt->p_slope_large_pos */ + int16_t init_p_slope_large_neg; /* cal+0x11C — copied to rt->p_slope_large_neg */ + int16_t init_integ_step_large_pos;/* cal+0x120 — copied to rt->integ_step_large_pos */ + int16_t init_integ_step_large_neg;/* cal+0x122 — copied to rt->integ_step_large_neg */ + int16_t init_open_loop_p_gain; /* cal+0x124 byte, clamped to 15 — copied to rt->open_loop_p_gain */ - /* RPM-window matching for pwm_period slew (ROM 0x538b-0x5575). - * Eight breakpoints at cal+0xF2 define four bands (lo,hi); the - * cal+0x102 halfwidth adds hysteresis. RPM INSIDE any band drives - * pwm_period toward pwm_period_min (→ non-zero shape contribution - * adds ~49 duty ticks). RPM OUTSIDE all bands drives pwm_period - * toward pwm_period_max (→ zero contribution; Y-table floor). */ - int16_t pwm_rpm_windows[8]; /* cal+0xF2 — 4 (lo,hi) pairs */ - int16_t pwm_window_halfwidth; /* cal+0x102 — hysteresis halfwidth */ - /* Per-cycle slew step magnitude for pwm_period. Sourced from - * cal+0x104 (= 354); cached to RAM[0x02ec] at FUN_5314:0x53b9 and - * read at the Phase 1 and Phase 3 sites. Aliases pwm_const_104 — - * same offset, semantic name. */ - int16_t pwm_slew_step; /* cal+0x104 — slew magnitude */ + /* CL correction normalizers (RAM in ROM, cal-resident here for parity). */ + int16_t init_pos_error_normalizer; + int16_t init_neg_error_normalizer; - const int16_t *pwm_y_table; /* cal+0x154 (indirect) */ - const int16_t *shape_y_table; /* cal+0x15E (indirect) */ + /* PWM stage scalars */ + int16_t pwm_detail_x0; + int16_t pwm_detail_x1; + int16_t pwm_cached_ptr_0F2; + int16_t pwm_cached_ptr_102; + int16_t pwm_const_104; - int16_t closed_loop_gain_const; /* cached at ROM 0x6056 = 0x000A */ + int16_t pwm_rpm_windows[8]; + int16_t pwm_window_halfwidth; + int16_t pwm_slew_step; - uint16_t pwm_period_min; /* hypothesised; see cal_tables_rom.c */ - uint16_t pwm_period_max; /* hypothesised */ + const int16_t *pwm_y_table; + const int16_t *shape_y_table; - /* PWM duty clamp bounds — RAM[0x6058]/RAM[0x605a] flash mirrors. - * Defaults 0x00CD/0x0F32 match the default family's pwm_min/pwm_max. */ - uint16_t pwm_min; /* RAM[0x6058] cache */ - uint16_t pwm_max; /* RAM[0x605a] cache */ + int16_t closed_loop_gain_const; + + uint16_t pwm_period_min; + uint16_t pwm_period_max; + uint16_t pwm_min; + uint16_t pwm_max; }; /* ── Submap descriptor ──────────────────────────────────────────────── */ 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 flags; + const int16_t *input_ptr; + uint16_t count; + const int16_t *x; uint16_t input_addr; }; -/** Bind descriptor input_ptr fields to rt inputs. */ static inline void pwm_bind_submap_inputs( pwm_runtime_t *rt, pwm_submap_descr_t *descrs, @@ -319,15 +266,8 @@ static inline void pwm_bind_submap_inputs( * PUBLIC API * ══════════════════════════════════════════════════════════════════════ */ -/** pwm_flash_t — placeholder for family-1 API parity. Family-1 has Y-tables - * in a separate `pwm_flash_t` struct; t06211 keeps them inside - * pwm_calibration_t so this struct is empty. Kept so FBKW.c / callers - * can pass &pwm_flash_rom without the call failing. */ typedef struct pwm_flash { char _unused; } pwm_flash_t; -/** 4-argument init matching family-1's pwm_init signature. The `flash` - * pointer is accepted (must be non-NULL — pass &pwm_flash_rom) and - * ignored by the t06211 pipeline. */ void pwm_init(pwm_runtime_t *rt, const pwm_calibration_t *cal, const pwm_flash_t *flash, @@ -335,44 +275,17 @@ void pwm_init(pwm_runtime_t *rt, void pwm_service(pwm_runtime_t *rt); -/** @brief CKP-edge reset hook — call from the CKP interrupt handler. - * - * Sets rt->reset_flag = 1 so the next pwm_service() call zeroes the - * closed-loop accumulator (supervisor_state_17e) and the enable counter - * (cl_enable_counter). Byte store is atomic on all mainstream targets; - * no locking required as long as it is not called concurrently with - * pwm_service(). - * - * Pattern parity with the default family. See - * docs/re-guide-ckp-reset-pattern.md for the full rationale. - * - * **Variant note for t06211:** the reset_flag producer is **absent from - * the ROM** (no STB #1 site writing to 0x002e). The supervisor at 0x7beb - * still consumes reset_flag == 1 and clears it, but nothing inside the - * ROM ever sets it. Consequence: the accumulator walk-off hazard is - * structurally identical to family-1, and the application layer must - * drive this hook itself at engine-rev rate (typically 4 pulses per - * revolution on VP44, ~12–15 scheduler cycles apart at 1200 rpm with a - * 1 kHz scheduler). Without it, supervisor_state_17e integrates forever. - */ static inline void pwm_ckp_isr(pwm_runtime_t *rt) { rt->reset_flag = 1; } -/** Utility: 1D descending-X piecewise-linear lookup. */ int16_t pwm_interp_lookup(const int16_t *x, const int16_t *y, uint16_t n, int16_t in); -/** Utility: bypass-PI bilinear LUT — eval(pwm_A, rpm), eval(pwm_B, fbkw), - * combine over Y-table, then apply [205, 3890] clamp. Use this to query - * the ROM Y-table directly as a (rpm, fbkw) lookup, skipping the PI - * controller entirely. Returns the clamped duty. */ uint16_t pwm_lut_duty(const pwm_calibration_t *cal, uint16_t rpm, int16_t fbkw); -/* ── ROM-decoded cal + flash placeholder (defined in cal_tables_rom.c) ── */ extern const pwm_calibration_t pwm_cal_rom; extern const pwm_flash_t pwm_flash_rom; -/** Descriptor array indices. */ enum pwm_submap_id { PWM_SUBMAP_SETPOINT_INTERP = 0, PWM_SUBMAP_PWM_A = 1, diff --git a/Core/CAN_Libs/can_db.c b/Core/CAN_Libs/can_db.c index ff5e451..ac42409 100644 --- a/Core/CAN_Libs/can_db.c +++ b/Core/CAN_Libs/can_db.c @@ -193,7 +193,7 @@ const size_t CAN_ANSWERS_COUNT = sizeof(CAN_ANSWERS)/sizeof(CAN_ANSWERS[0]); static const uint8_t STARTUP_PAYLOAD_2[8] = {0x81, 0x7F, 0x1C, 0x38, 0x1C, 0xFC, 0xF5, 0x71}; static const uint8_t EPS_0024_PAYLOAD[8] = {0x64, 0x01, 0x01, 0x00, 0x00, 0x00, 0x5B, 0x37}; //0x75 immo 1 0x71 immo 0 static const uint8_t EPS_00B2_PAYLOAD[8] = {0x64, 0x01, 0x01, 0x00, 0x00, 0x21, 0x5B, 0x37};*/ -#elif defined(T06211) +#elif defined(T06211) || defined(T06215) static const uint8_t STARTUP_PAYLOAD[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; //0x75 immo 1 0x71 immo 0 #elif defined(T06209) || defined(T06216) diff --git a/Core/CAN_Libs/can_manager.c b/Core/CAN_Libs/can_manager.c index f0290d1..b58ce21 100644 --- a/Core/CAN_Libs/can_manager.c +++ b/Core/CAN_Libs/can_manager.c @@ -114,7 +114,6 @@ void can_manager_set_startup_reply_msg(const CanMessageDef *reply_msg) { s_startup_reply_msg = reply_msg; } -uint8_t startup = 1; /* void can_manager_rx_startup_trigger(const CanMessageDef *msg, const uint8_t in[8], CanTxFn tx) @@ -144,7 +143,9 @@ uint8_t startup_finished = 0; #if defined(T06301) uint8_t startup_count = 9; uint8_t startupiscar = 0; -#elif defined(T06209) || defined(T06216) || defined(T06211) +uint8_t startup = 1; + +#elif defined(T06209) || defined(T06216) || defined(T06211) || defined(T06215) uint8_t startup_count = 0; uint8_t startup_sent_count = 0; @@ -175,7 +176,7 @@ void can_manager_rx_boot_reply_always(const CanMessageDef *msg, const uint8_t in Timeout_StartIfStopped(20, TIM16->CNT - 500); //no deberia hacer na esto pq bootea started. } -#elif defined(T06209) || defined(T06216) || defined(T06211) +#elif defined(T06209) || defined(T06216) || defined(T06211) || defined(T06215) if (!startup_sent_count){return;} if(startup_finished){ diff --git a/Core/Inc/fuel_map.h b/Core/Inc/fuel_map.h index 99c8c2a..1450976 100644 --- a/Core/Inc/fuel_map.h +++ b/Core/Inc/fuel_map.h @@ -1,56 +1,11 @@ -/* - * fuel_map.h - * - * Created on: Oct 18, 2024 - * Author: herli - */ - -#ifndef FUEL_MAP_H_ -#define FUEL_MAP_H_ -#include -#include - -//extern float PHIAD_TEIN; -// Function declarations -extern void init_FuelMap(float* PHIAD); - -float GetAlpha(float RPM, float ME, float Tein, float Temp); -float GetBeta(float RPM, float Tein); +#ifndef FUEL_MAP_T06215_H +#define FUEL_MAP_T06215_H +/* Thin wrapper over the T06215 reverse-engineered phi pipeline. + * Preserves the legacy host-app API (init_FuelMap / FM_GET_PHIAD); + * underneath, every call drives phi_service and returns + * angle_accumulator (RW52) in float degrees. */ +void init_FuelMap(float *PHIAD); float FM_GET_PHIAD(float RPM, float ME, float Temp); -struct AlphaStruct { - //float ME_RPM_Beta_array[N_ME][N_RPM]; - float ME_RPM_Beta_array[FM_N_RPM][FM_N_ME]; //para la transpuesta -}; -extern struct AlphaStruct fuelmap_m12; -extern struct AlphaStruct fuelmap_m5; -extern struct AlphaStruct fuelmap_10; -extern struct AlphaStruct fuelmap_25; -extern struct AlphaStruct fuelmap_60; -//extern struct AlphaStruct fuelmap_80; -extern struct fuelMapIndexes fuelMapI; - -static const struct AlphaStruct* g_FuelMaps[] = { - &fuelmap_m12, - &fuelmap_m5, - &fuelmap_10, - &fuelmap_25, - &fuelmap_60, - //&fuelmap_80 - - // add/remove as needed; order must match fuelMapI.T_Index_array -}; - -struct fuelMapIndexes { - float RPM_Index_array[FM_N_RPM]; - float ME_Index_array[FM_N_ME]; - float T_Index_array[FM_N_T]; -}; - -// Declare the lookup table -//extern struct AlphaStruct fuelmap; - -extern float BoostMultiplier(uint8_t mode, float RPM, float ME); - -#endif /* FUEL_MAP2_H_ */ +#endif /* FUEL_MAP_T06215_H */ diff --git a/Core/Inc/id.h b/Core/Inc/id.h index 7797431..c9cf6ff 100644 --- a/Core/Inc/id.h +++ b/Core/Inc/id.h @@ -12,13 +12,13 @@ /* DEBUG PARAMETERS*/ //#define T06301 //ford 004 -> 002 004 006 || 504 -> 010 018 -//#define T06215 //bmw rover 004 -> 005 014 015 016 017 || 504 -> 005 007 017 || 006 -> 001 002 003 004 007 008 +#define T06215 //bmw rover 004 -> 005 014 015 016 017 || 504 -> 005 007 017 || 006 -> 001 002 003 004 007 008 //#define T15021 //audi 506 -> 030 033 //#define T31804 //audi 506 -> 037 038 //#define T06209 //eq: T06216 -#define T06211 +//#define T06211 -#define _504012 +#define _424026 /* FORD */ #define FORD_SYNC_PULSE_OUT 0 @@ -29,22 +29,19 @@ #define CYLINDERS 4 /* TIMING COMPENSATIONS */ -#define PHI1 41.016 +#define PHI1 41.0f -#define TEIN_NOMINAL 1550 +#define TEIN_NOMINAL 1556 //(phiad - injangle with fault tein)*totime = teinnom #define TEIN_FAULT 950 -#define FBKW_FEEDBACK_ZERO 53.75 +//#define FBKW_FEEDBACK_ZERO 45.2 +#define FBKW_FEEDBACK_ZERO 53.613 + #define FBKW_FEEDBACK_MIN -5.367 #define FBKW_FEEDBACK_MAX 21.27 -#define FBKW_FEEDBACK_IC_DT 27 - - - -#define FBKW_MAX 90 //en 504 parece que era 506 -#define FBKW_MAX_REAL_DEM 165 //wtf is this +#define FBKW_FEEDBACK_IC_DT 18 /* CAN DEFINITIONS */ #define CAN_BAUDRATE 500 diff --git a/Core/Inc/id_424026.h b/Core/Inc/id_424026.h new file mode 100644 index 0000000..c9cf6ff --- /dev/null +++ b/Core/Inc/id_424026.h @@ -0,0 +1,91 @@ +/* + * id.h + * + * Created on: Jul 28, 2025 + * Author: herli + */ + +#ifndef INC_ID_H_ +#define INC_ID_H_ + +/* 504 012 */ + +/* DEBUG PARAMETERS*/ +//#define T06301 //ford 004 -> 002 004 006 || 504 -> 010 018 +#define T06215 //bmw rover 004 -> 005 014 015 016 017 || 504 -> 005 007 017 || 006 -> 001 002 003 004 007 008 +//#define T15021 //audi 506 -> 030 033 +//#define T31804 //audi 506 -> 037 038 +//#define T06209 //eq: T06216 +//#define T06211 + +#define _424026 + +/* FORD */ +#define FORD_SYNC_PULSE_OUT 0 + +#define ENABLE_AUDI_IMMO 0 +#define HAS_PREINJECTION 0 + +#define CYLINDERS 4 + +/* TIMING COMPENSATIONS */ +#define PHI1 41.0f + +#define TEIN_NOMINAL 1556 +//(phiad - injangle with fault tein)*totime = teinnom +#define TEIN_FAULT 950 + +//#define FBKW_FEEDBACK_ZERO 45.2 +#define FBKW_FEEDBACK_ZERO 53.613 + +#define FBKW_FEEDBACK_MIN -5.367 +#define FBKW_FEEDBACK_MAX 21.27 + +#define FBKW_FEEDBACK_IC_DT 18 + +/* CAN DEFINITIONS */ +#define CAN_BAUDRATE 500 +#define CAN_RPM_SEND_ASYNC 250 +#define CAN_EMPF2_INSTANT 0 + +/* ALL FUELMAP */ +#define FM_N_RPM 6 +#define FM_N_ME 11 +#define FM_N_T 5 + + +/* PEAK AND HOLD */ +#define PH_PEAK_DEF 600 + +/* ANALOG CALIBRATION PARAMETERS */ +// MOSFET +#define V_PEAK 2.94 +#define V_HOLD 1.94 + +#define INJ_CLOSING_MARGIN 0 //o cero, o 20 + +// TOOTHED WHEEL +#define TW_MT_THRESHOLD 2.4 +#define TW_THEETHS 120 +#define TW_TOOTH_ALPHA 3 +#define TW_STARTED_RPM 400 + +#define MIN_RPM 20 + +#define USTODEG 0.000006 +// TIMINGS +#define TEIN_READING_OFFSET 8 + +/* ifdef things */ + +#if CYLINDERS == 4 + #define TW_PERCYL_TEETH 26 +#elif CYLINDERS == 6 + #define TW_PERCYL_TEETH 16 +#else + #define TW_PERCYL_TEETH 26 + #error "Unsupported number of cylinders" +#endif + + +#endif /* INC_ID_H_ */ diff --git a/Core/Kline_Libs/psg_prop.h b/Core/Kline_Libs/psg_prop.h index 1c484dd..dc2202c 100644 --- a/Core/Kline_Libs/psg_prop.h +++ b/Core/Kline_Libs/psg_prop.h @@ -52,6 +52,12 @@ #define PSG_KUNDENNUMMER1_STR "167005M321" #define PSG_KUNDENNUMMER2_STR " " #define PSG_MOD_INDEX_STR "000001" +#elif defined(_424026) +#define PSG_IDENT_STR "1093424026" +#define PSG_KUNDENNUMMER_STR "167002X900 " +#define PSG_KUNDENNUMMER1_STR "167002X900" +#define PSG_KUNDENNUMMER2_STR " " +#define PSG_MOD_INDEX_STR "000001" #else /* Modification index — 6 ASCII chars, ROM 0x93A7 */ diff --git a/Core/Phi/compute_temp_phi_comp.c b/Core/Phi/compute_temp_phi_comp.c new file mode 100644 index 0000000..88f44a3 --- /dev/null +++ b/Core/Phi/compute_temp_phi_comp.c @@ -0,0 +1,142 @@ +#include "compute_temp_phi_comp.h" + +#include + +/* + * 1:1 translation of FUN_6b2a @ 0x6b2a + FUN_6b4e @ 0x6b4e for the + * T06215 variant. Per-cylinder dataflow notes live in + * compute_temp_phi_comp.h. + * + * MCS-96 frame: in the ROM the IIR step is reached via + * `Timer_1khz:79e2 LDB R94, DAT_6061` followed by `R94 -= 1; if (!R94) { ... }` + * around 0x79dc. R94 reloads from DAT_6061 = 0x64 = 100 (`Timer_1khz:79e9`, + * also seen at FUN_7a1c:7a26). The compact port keeps R94 file-static so + * the host's only contract is "call once per 1 ms". + */ + +#define TEMP_PHI_COMP_R94_RELOAD ((uint8_t)100) /* DAT_6061 = 0x64 */ + +/* Mirrors the R94 byte register slot. Boot value matches FUN_7a1c:7a26 + * `LDB R94, DAT_6061`; phi_init re-asserts it via compute_temp_phi_comp_init. */ +static uint8_t s_r94 = TEMP_PHI_COMP_R94_RELOAD; + +void compute_temp_phi_comp_init(void) +{ + s_r94 = TEMP_PHI_COMP_R94_RELOAD; +} + +/* + * 1:1 port of FUN_6b4e @ 0x6b4e. + * + * 0x6b4e: MULU RL1C, RW9C, CAL[0x84] ; RL1C = RW9C *u CAL[0x84] + * 0x6b54: ST RW1E, RW9C ; RW9C := high(RW9C *u CAL[0x84]) + * 0x6b57: MULU RL1C, RW20, CAL[0x84] ; RL1C = RW20 *u CAL[0x84] + * 0x6b5d: ADD RW9C, RW1C ; RW9C += low(RW20 *u CAL[0x84]); sets C + * 0x6b60: ADDC RW1E, ZR ; RW1E += 0 + C + * 0x6b63: LD RW20, RW1E ; RW20 := high(RW20 *u CAL[0x84]) + carry + * 0x6b66: MULU RL1C, RW9A, CAL[0x82] ; RL1C = RW9A *u CAL[0x82] + * 0x6b6c: ADD RW9C, RW1C ; RW9C += low(RW9A *u CAL[0x82]); sets C + * 0x6b6f: ADDC RW20, RW1E ; RW20 += high(RW9A *u CAL[0x82]) + C + * 0x6b72: RET + * + * The trace operates on the {RW20:RW9C} pair where RW20 is the caller's + * working high-word copy (sign-flipped or not by the FUN_6b2a wrapper). + * All multiplies are unsigned (MULU); all carries propagate exactly as + * the ADD/ADDC chain in the assembly. + */ +static void iir_step_unsigned(uint16_t *rw20, + uint16_t *rw9c, + uint16_t rw9a, + uint16_t cal_82, + uint16_t cal_84) +{ + uint16_t rw1c, rw1e; + uint32_t prod; + + /* 0x6b4e + 0x6b54: rw9c becomes the HIGH word of (rw9c * a). */ + prod = (uint32_t)*rw9c * (uint32_t)cal_84; + rw1e = (uint16_t)(prod >> 16); + *rw9c = rw1e; + + /* 0x6b57: prod = rw20 * a; rw1c = low, rw1e = high. */ + prod = (uint32_t)*rw20 * (uint32_t)cal_84; + rw1c = (uint16_t)(prod & 0xFFFFu); + rw1e = (uint16_t)(prod >> 16); + + /* 0x6b5d / 0x6b60: ADD rw9c, rw1c — capture carry, propagate to rw1e. */ + { + uint32_t s = (uint32_t)*rw9c + rw1c; + *rw9c = (uint16_t)(s & 0xFFFFu); + rw1e = (uint16_t)(rw1e + (s >> 16)); + } + + /* 0x6b63: rw20 := rw1e (= high(rw20*a) + carry). */ + *rw20 = rw1e; + + /* 0x6b66: prod = rw9a * b. */ + prod = (uint32_t)rw9a * (uint32_t)cal_82; + rw1c = (uint16_t)(prod & 0xFFFFu); + rw1e = (uint16_t)(prod >> 16); + + /* 0x6b6c / 0x6b6f: ADD rw9c, rw1c — propagate carry to rw20. */ + { + uint32_t s = (uint32_t)*rw9c + rw1c; + *rw9c = (uint16_t)(s & 0xFFFFu); + *rw20 = (uint16_t)(*rw20 + rw1e + (uint16_t)(s >> 16)); + } +} + +/* + * 1:1 port of FUN_6b2a @ 0x6b2a — sign-aware wrapper around the unsigned IIR step. + * + * 0x6b30: LD RW20, RW9E + * 0x6b33: JBC R21, 0x7, 0x6b42 ; bit 7 of R21 = sign bit of RW20 (= RW9E) + * 0x6b36: NEG RW9C ; RW9C := -RW9C (two's complement) + * 0x6b38: NOT RW20 ; RW20 := ~RW9E (one's complement) + * 0x6b3a: SCALL FUN_6b4e + * 0x6b3c: NOT RW20 ; un-negate + * 0x6b3e: NEG RW9C ; un-negate + * 0x6b40: SJMP 0x6b44 + * 0x6b42: SCALL FUN_6b4e ; RW9E >= 0 path: run IIR step directly + * 0x6b44: LD RW9E, RW20 ; commit new high word + * + * The pre/post (NEG, NOT) pair is two's-complement negation of the + * {RW9E:RW9C} pair when RW9C != 0, and ~RW9E (off-by-one) when RW9C == 0. + * That asymmetry is preserved verbatim — it's the ROM's actual behaviour. + */ +void compute_temp_phi_comp_tick_1khz(runtime_state_t *rt, + const calibration_t *cal) +{ + /* Mirrors the Timer_1khz prescaler at 0x79e2: + * R94 = R94 - 1; if (R94 == 0) { run; R94 = DAT_6061; } */ + if (--s_r94 != 0u) { + return; + } + s_r94 = TEMP_PHI_COMP_R94_RELOAD; + + /* 0x6b30: RW20 = RW9E. */ + uint16_t rw20 = (uint16_t)rt->rw9e; + uint16_t rw9c = (uint16_t)rt->rw9c; + uint16_t rw9a = (uint16_t)rt->rw9a; + uint16_t cal_82 = (uint16_t)cal->cal_82; + uint16_t cal_84 = (uint16_t)cal->cal_84; + + if ((int16_t)rt->rw9e < 0) { + /* 0x6b36 / 0x6b38: NEG RW9C, NOT RW20. */ + rw9c = (uint16_t)(-(int16_t)rw9c); + rw20 = (uint16_t)~rw20; + + iir_step_unsigned(&rw20, &rw9c, rw9a, cal_82, cal_84); + + /* 0x6b3c / 0x6b3e: NOT RW20, NEG RW9C. */ + rw20 = (uint16_t)~rw20; + rw9c = (uint16_t)(-(int16_t)rw9c); + } else { + /* 0x6b42: direct call when RW9E sign bit clear. */ + iir_step_unsigned(&rw20, &rw9c, rw9a, cal_82, cal_84); + } + + /* 0x6b44: commit. */ + rt->rw9e = (int16_t)rw20; + rt->rw9c = (int16_t)rw9c; +} diff --git a/Core/Phi/compute_temp_phi_comp.h b/Core/Phi/compute_temp_phi_comp.h new file mode 100644 index 0000000..6f1c5b0 --- /dev/null +++ b/Core/Phi/compute_temp_phi_comp.h @@ -0,0 +1,51 @@ +#ifndef COMPUTE_TEMP_PHI_COMP_H +#define COMPUTE_TEMP_PHI_COMP_H + +#include "phi.h" + +/* + * compute_temp_phi_comp — 1:1 translation of FUN_6b2a @ 0x6b2a (sign-aware + * wrapper) + FUN_6b4e @ 0x6b4e (unsigned 32-bit IIR step) for the T06215 + * variant. + * + * Sole producer of `rt->rw9e` (RW9E @ 0x009E), the high word of the signed + * 32-bit IIR-filter accumulator `{rt->rw9e:rt->rw9c}`. The IIR low-passes + * the rpm-derived term `rt->rw9a` (already produced by + * `compute_temp_comp_factor` at 0x6AF9). `rt->rw9e` is consumed by the + * same `compute_temp_comp_factor` at 0x6AFC to compute the dynamic + * temperature-compensation factor `rt->temp_comp_factor` that scales the + * 2-D angle kick. + * + * Cadence: in the ROM the IIR step runs from `Timer_1khz` (1 kHz) gated + * by an R94 prescaler that reloads from `DAT_6061 = 0x64 = 100`, so the + * step actually fires at 10 Hz. This compact port preserves both layers: + * the host calls `compute_temp_phi_comp_tick_1khz` every 1 ms, and the + * file-static R94 prescaler in `compute_temp_phi_comp.c` gates the IIR + * step internally. The host does not need to know about the 10 Hz inner + * rate. + * + * Coefficients (per-variant calibration, Q16 unsigned fractions): + * - cal->cal_82 — input gain `b` (CAL[RWA4+0x82]). + * - cal->cal_84 — pole `a` (CAL[RWA4+0x84]). + * On this ROM `cal_82 + (uint16_t)cal_84 == 0x10000`, giving unity DC gain. + * + * Boot semantics: `phi_init` already memsets `rt->rw9c` / `rt->rw9e` to 0 + * (mirrors `FUN_6ba3` @ 0x6bbf–0x6bc1). The R94 prescaler is reset by + * `compute_temp_phi_comp_init`, called from `phi_init` (mirrors + * `Timer_1khz` LDB R94, DAT_6061 at 0x7a26). + */ + +/** Reset the file-static R94 prescaler to its boot value (100). Called + * once from `phi_init`. Safe to call again to re-arm after a soft reset. */ +void compute_temp_phi_comp_init(void); + +/** 1 kHz hook. The host invokes this exactly once every 1 ms from its + * millisecond timer. Internally decrements the file-static R94; on + * R94 == 0 runs the IIR step (port of FUN_6b2a + FUN_6b4e) and reloads + * R94 to 100. The IIR updates `{rt->rw9e, rt->rw9c}` in place using + * `rt->rw9a` as the input and `cal->cal_82` / `cal->cal_84` as + * coefficients. */ +void compute_temp_phi_comp_tick_1khz(runtime_state_t *rt, + const calibration_t *cal); + +#endif /* COMPUTE_TEMP_PHI_COMP_H */ diff --git a/Core/Phi/phi.c b/Core/Phi/phi.c new file mode 100644 index 0000000..9533a3e --- /dev/null +++ b/Core/Phi/phi.c @@ -0,0 +1,1210 @@ +/** + * @file phi.c + * @brief T06215 compact-port — monolithic single-translation-unit + * implementation of the injection-angle producer chain. + * + * All seven producer stages and the shared submap helpers are inlined + * directly into this file as `static` functions (1:1 translations of + * the verbose modules under variants/T06215/src/, brought along + * verbatim so per-block disassembly-address comments remain + * authoritative). The producer itself (s_final_injection_angle @ + * 0x754D) is byte-equivalent to T06211's FUN_7453 — see + * variants/T06215/README.md — so the inlined bodies and assembly + * address ranges are shared with T06211. The call order in + * `phi_service` matches T06211's fused-scheduler walk + * (variants/T06211/docs/open-questions.md §4 / §5). + * + * Pipeline (Stages 2–7 in `phi_service`): + * 2. compute_tein_overtemp_guard — port of FUN_5ca1 (RW7A seed addend) + * 3. compute_accel_comp_gain — port of FUN_72b0 (3-submap pipeline → *(0x0164)) + * 3b. compute_angle_accumulator_3d — port of FUN_722e (3-D trilinear → RW52) + * 4. compute_accel_comp_offset — port of FUN_732d (Δ-rpm × gain → RW3C, sets REC.0) + * 5. compute_gate_0220 — port of orphan @ 0x77ff (RPM hysteresis) + * 6. compute_tein_valve_fault_guard — port of FUN_62a2 (Phase-1/2 byte state machine → RW7A) + * 7. compute_target_injection_angle — port of FUN_7453 (sole writer of RW48 / RW5A) + * + * Every MCS-96 arithmetic choice (MUL vs MULU, SHRA vs SHR logical, JGE + * vs JC) mirrors the disassembly — see the per-block comments with + * disassembly address ranges, and variants/T06215/src/ for the + * long-form function-level commentary these stages were ported from. + */ +#include "phi.h" + +#include + +/* ══════════════════════════════════════════════════════════════════════ + * Internal helpers — submap infrastructure + * + * 1:1 translations of the four shared helpers that the producer stages + * below call (eval_submap_to_scratch, combine_two_submaps_to_word, + * combine_three_submaps_to_word, refine_submap_result), plus two + * file-static utilities (bytes_to_words, bilinear_at_plane). + * + * Brought verbatim from variants/T06215/src/submap_eval.c with + * external linkage demoted to `static`. + * ══════════════════════════════════════════════════════════════════════ */ + +/* + * 1:1 translation of eval_submap_to_scratch @ 0x81db–0x8236. + * + * Populates scratch with {stride_bytes, delta, fraction, index_bytes} + * so downstream interpolators can locate the input within the + * (descending-ordered) axis and linearly interpolate between cells. + */ +static void eval_submap_to_scratch(const submap_descriptor_t *desc, + submap_scratch_t *scratch) +{ + /* 0x81e3–0x81e7: read the input variable via the descriptor's pointer */ + int16_t input_val = *desc->input_var; + + /* 0x81ea–0x81f1: stride_bytes = stride_items << 1 */ + int16_t stride_bytes = (int16_t)((uint16_t)desc->stride_items << 1); + scratch->stride_bytes = stride_bytes; + + const int16_t *axis = desc->axis; + + /* 0x81fc–0x81ff: compare with axis[0]; if input >= axis[0] take the + * edge case that places the interpolator at axis[0]. */ + if (input_val >= axis[0]) { + /* 0x8226–0x8232: delta=2, fraction=2, index_bytes=2 */ + scratch->delta = 2; + scratch->fraction = 2; + scratch->index_bytes = 2; + return; + } + + /* 0x8201–0x8204: descending scan. Loop while input < axis[i]. + * Exit condition: input >= axis[i]; i.e. input falls in + * [axis[i], axis[i-1]). We already know input < axis[0]. */ + int16_t i = 1; + while (input_val < axis[i]) { + i++; + /* Calibration invariant: axis[stride_items-1] == 0 and input >= 0, + * so the loop terminates before running off the end. */ + } + + /* 0x820a–0x820e: delta = axis[i-1] − axis[i] (positive on descending) */ + scratch->delta = (int16_t)(axis[i - 1] - axis[i]); + /* 0x8215–0x8218: fraction = input − axis[i] (0 ≤ fraction ≤ delta) */ + scratch->fraction = (int16_t)(input_val - axis[i]); + /* 0x821c–0x8221: index_bytes = byte offset from axis[0] = i × 2 */ + scratch->index_bytes = (int16_t)(i * 2); +} + +/* + * Byte-offset helper: the MCS-96 treats scratch index_bytes and + * stride_bytes as byte offsets but the C pointer arithmetic here + * is on int16_t (word) arrays. All byte offsets are guaranteed + * even by the producer, so dividing by 2 is exact. + */ +static inline int32_t bytes_to_words(int16_t byte_offset) +{ + /* signed arithmetic: SHRAL in the assembly is sign-preserving */ + return (int32_t)byte_offset / 2; +} + +/* + * 1:1 translation of combine_two_submaps_to_word @ 0x8258–0x82b4. + * + * 2-D bilinear interpolation. scratch_x = inner axis (per-row), + * scratch_y = outer axis (between-row step = stride_x_bytes). + */ +static int16_t combine_two_submaps_to_word(const int16_t *data_table, + const submap_scratch_t *scratch_x, + const submap_scratch_t *scratch_y) +{ + /* 0x8264–0x826f: compute byte offset to (X, Y_lo) cell. + * y_off_bytes = (idx_y × stride_x_bytes) / 2 (signed MUL + SHRAL) + * total_off = y_off_bytes + idx_x_bytes + */ + int32_t y_prod = (int32_t)scratch_y->index_bytes * (int32_t)scratch_x->stride_bytes; + int16_t y_off_bytes = (int16_t)(y_prod >> 1); + int16_t total_off_ylo = (int16_t)(y_off_bytes + scratch_x->index_bytes); + int32_t word_idx_ylo = bytes_to_words(total_off_ylo); + + /* 0x8276–0x828a: lerp X within Y_lo row. + * upper = data[idx-1] (descending → prev word is higher axis value) + * lower = data[idx] + * lerp = lower + (upper − lower) × frac_x / delta_x + */ + int16_t lower_ylo = data_table[word_idx_ylo]; + int16_t upper_ylo = data_table[word_idx_ylo - 1]; + int32_t dx_ylo = (int32_t)(int16_t)(upper_ylo - lower_ylo) * (int32_t)scratch_x->fraction; + int16_t lerp_ylo = (int16_t)((int16_t)(dx_ylo / scratch_x->delta) + lower_ylo); + + /* 0x828d: step to Y_hi (one row back, i.e. −stride_x_bytes) */ + int32_t word_idx_yhi = word_idx_ylo - bytes_to_words(scratch_x->stride_bytes); + + /* 0x8290–0x82a1: lerp X within Y_hi row */ + int16_t lower_yhi = data_table[word_idx_yhi]; + int16_t upper_yhi = data_table[word_idx_yhi - 1]; + int32_t dx_yhi = (int32_t)(int16_t)(upper_yhi - lower_yhi) * (int32_t)scratch_x->fraction; + int16_t lerp_yhi = (int16_t)((int16_t)(dx_yhi / scratch_x->delta) + lower_yhi); + + /* 0x82a4–0x82b1: lerp Y between lerp_yhi and lerp_ylo */ + int32_t dy = (int32_t)(int16_t)(lerp_yhi - lerp_ylo) * (int32_t)scratch_y->fraction; + return (int16_t)((int16_t)(dy / scratch_y->delta) + lerp_ylo); +} + +/* + * Helper: 2-D bilinear at a fixed Z plane. Factored so the 3-D version + * below reads clearly. base_word_idx points to (X_lo, Y_lo) within the + * chosen Z plane. + */ +static int16_t bilinear_at_plane(const int16_t *data_table, + int32_t base_word_idx, + const submap_scratch_t *sx, + const submap_scratch_t *sy) +{ + int16_t lower_ylo = data_table[base_word_idx]; + int16_t upper_ylo = data_table[base_word_idx - 1]; + int32_t dx_ylo = (int32_t)(int16_t)(upper_ylo - lower_ylo) * (int32_t)sx->fraction; + int16_t lerp_ylo = (int16_t)((int16_t)(dx_ylo / sx->delta) + lower_ylo); + + int32_t yhi = base_word_idx - bytes_to_words(sx->stride_bytes); + int16_t lower_yhi = data_table[yhi]; + int16_t upper_yhi = data_table[yhi - 1]; + int32_t dx_yhi = (int32_t)(int16_t)(upper_yhi - lower_yhi) * (int32_t)sx->fraction; + int16_t lerp_yhi = (int16_t)((int16_t)(dx_yhi / sx->delta) + lower_yhi); + + int32_t dy = (int32_t)(int16_t)(lerp_yhi - lerp_ylo) * (int32_t)sy->fraction; + return (int16_t)((int16_t)(dy / sy->delta) + lerp_ylo); +} + +/* + * 1:1 translation of FUN_82b5 @ 0x82b5–0x838a — 3-D trilinear combine. + * Inner = A (X), middle = B (Y), outer = C (Z). + */ +static int16_t combine_three_submaps_to_word(const int16_t *data_table, + const submap_scratch_t *scratch_a, + const submap_scratch_t *scratch_b, + const submap_scratch_t *scratch_c) +{ + /* 0x82c5–0x82cc: stride_per_Cplane (bytes) = stride_a × stride_b / 2 */ + int32_t prod_ab = (int32_t)scratch_a->stride_bytes * (int32_t)scratch_b->stride_bytes; + int16_t stride_cplane_bytes = (int16_t)(prod_ab >> 1); + + /* 0x82d1–0x82d6: Z byte offset = stride_cplane × idx_c / 2 */ + int32_t prod_z = (int32_t)stride_cplane_bytes * (int32_t)scratch_c->index_bytes; + int16_t z_off_bytes = (int16_t)(prod_z >> 1); + + /* 0x82d9–0x82e1: Y byte offset = idx_b × stride_a / 2 */ + int32_t prod_y = (int32_t)scratch_b->index_bytes * (int32_t)scratch_a->stride_bytes; + int16_t y_off_bytes = (int16_t)(prod_y >> 1); + + /* 0x82e4–0x82eb: base offset for (X, Y, Z_low) */ + int16_t base_off_bytes = (int16_t)(y_off_bytes + scratch_a->index_bytes + z_off_bytes); + int32_t base_idx_clo = bytes_to_words(base_off_bytes); + + /* 0x82f0–0x832e: 2-D bilinear at C_low plane */ + int16_t v_clo = bilinear_at_plane(data_table, base_idx_clo, scratch_a, scratch_b); + + /* 0x8335: step back one C plane → C_hi */ + int32_t base_idx_chi = base_idx_clo - bytes_to_words(stride_cplane_bytes); + + /* 0x833a–0x8375: 2-D bilinear at C_hi plane */ + int16_t v_chi = bilinear_at_plane(data_table, base_idx_chi, scratch_a, scratch_b); + + /* 0x8378–0x8387: lerp C between v_chi and v_clo */ + int32_t dz = (int32_t)(int16_t)(v_chi - v_clo) * (int32_t)scratch_c->fraction; + return (int16_t)((int16_t)(dz / scratch_c->delta) + v_clo); +} + +/* + * 1:1 translation of refine_submap_result @ 0x8237–0x8257. + * + * The ROM walks the data table in BYTES via 0x6[scratch] (index_bytes), + * then takes -0x2[ptr] (the previous int16) as `upper` and *ptr as + * `lower`. In C, that's data_table[index_words] = lower and + * data_table[index_words - 1] = upper, where + * index_words = index_bytes / 2. + */ +static int16_t refine_submap_result(const int16_t *data_table, + const submap_scratch_t *scratch) +{ + int32_t index_words = bytes_to_words(scratch->index_bytes); + int16_t lower = data_table[index_words]; + int16_t upper = data_table[index_words - 1]; + int32_t prod = (int32_t)(int16_t)(upper - lower) * (int32_t)scratch->fraction; + int16_t quot = (int16_t)(prod / scratch->delta); + return (int16_t)(quot + lower); +} + +/* ══════════════════════════════════════════════════════════════════════ + * Producer stages — 1:1 ports of the seven verbose modules under + * variants/T06215/src/, brought along verbatim with external linkage + * demoted to `static`. Each docstring identifies the originating ROM + * function (FUN_xxxx @ 0xyyyy). + * ══════════════════════════════════════════════════════════════════════ */ + +/* + * Translation of FUN_5ca1 @ 0x5ca1–0x5d57 (T06211 variant), scoped to the + * tein_overtemp_guard producer. + * + * The ROM's prologue (0x5ca7–0x5cf6) is a sensor-raw temperature scaler: + * it reads *(0x0144), scales via MULU+ADD, range-validates against + * rom_6014/rom_6016, and either commits the result to *(0x0146) + * (temperature) or reloads from temperature on out-of-range. This C port + * treats `temperature` as the input boundary, so that scaler is omitted — + * `rt->temperature` is supplied externally and consumed directly. + * + * Peripheral status writes (RFE bits 0/7, RWEA bits 5/6) at 0x5cfe / + * 0x5d01 / 0x5d4a / 0x5d4d are NOT modelled — sensor-fault status bits + * to the diagnostics subsystem, no feedback into the angle pipeline. + */ +static void compute_tein_overtemp_guard(runtime_state_t *rt, const calibration_t *cal) +{ + /* 0x5ca1–0x5ca6: prologue (PUSH RW1C, RW1E, RW20) — handled by C locals */ + + /* 0x5ca7–0x5cf6 (sensor-raw temperature scaler) is scoped out — see + * function docstring. We pick up at 0x5cf7 with rw1e seeded directly + * from rt->temperature. */ + int16_t rw1c; + int16_t rw1e = rt->temperature; + + /* 0x5cf7–0x5cfd: SUB RW1E, CAL[RWA4]+0x92 ; JNH LAB_5d45 (UNSIGNED) + * If the subtraction underflows (temperature <= cal_92 unsigned), go + * to the reset path — tein_overtemp_guard is forced to 0. */ + if ((uint16_t)rw1e <= (uint16_t)cal->cal_92) { + goto reset_path; + } + rw1e = (int16_t)((uint16_t)rw1e - (uint16_t)cal->cal_92); + + /* 0x5cfe–0x5d04: RFE |= 0x80, RWEA |= 0x20 (peripheral; not modelled) */ + + /* 0x5d05–0x5d0a: MULU RL1C, RW1E, CAL[RWA4]+0x94 (3-op UNSIGNED 16×16→32) + * RL1C = rw1e × cal_94. Low word lands in rw1c, high word in rw1e. */ + { + uint32_t rl1c = (uint32_t)(uint16_t)rw1e * (uint32_t)(uint16_t)cal->cal_94; + rw1c = (int16_t)(uint16_t)(rl1c & 0xFFFFu); + rw1e = (int16_t)(uint16_t)(rl1c >> 16); + } + + /* 0x5d0b–0x5d16: CMP RW1C, CAL[RWA4]+0x96 ; JNH LAB_5d17 ; LD RW1C, cal_96 + * UNSIGNED upper-clamp. JNH = unsigned <=, so the JNH skips the + * clamp when rw1c <= cal_96; we clamp when rw1c > cal_96. */ + if ((uint16_t)rw1c > (uint16_t)cal->cal_96) { + rw1c = cal->cal_96; + } + + /* 0x5d17–0x5d19: LD RW20, RW40 — copy rpm into RW20 */ + uint16_t rw20 = rt->rpm; + + /* 0x5d1a–0x5d20: CMP RW20, CAL[RWA4]+0x98 ; JNH LAB_5d45 (UNSIGNED) + * If rpm <= cal_98 → reset (tein_overtemp_guard = 0). */ + if (rw20 <= (uint16_t)cal->cal_98) { + goto reset_path; + } + + /* 0x5d21–0x5d27: CMP RW20, CAL[RWA4]+0x9a ; JH LAB_5d3e (UNSIGNED) + * If rpm > cal_9a → use raw rw1c (no rpm scaling). */ + if (rw20 > (uint16_t)cal->cal_9a) { + goto store_011a; + } + + /* 0x5d28–0x5d3d: rpm-blend + * RW1E = rpm − cal_98 (3-op SUB; fresh write, no carryover) + * RL1C = rw1c × rw1e (2-op MULU) + * RW20 = cal_9a − cal_98 (3-op SUB) — actually 2-op rewrite + * RL1C /= rw20 (DIVU) + * Result: rw1c (quotient) is the rpm-scaled tein_overtemp_guard value. */ + { + uint16_t numer_b = (uint16_t)(rw20 - (uint16_t)cal->cal_98); + uint32_t rl1c = (uint32_t)(uint16_t)rw1c * (uint32_t)numer_b; + uint16_t denom = (uint16_t)((uint16_t)cal->cal_9a - (uint16_t)cal->cal_98); + if (denom == 0u) { + /* MCS-96 DIVU traps on zero divisor; defensively guard. + * Real ROM relies on cal_9a > cal_98 being a cal-block + * invariant (the unsigned tests above guarantee we only + * reach here when cal_98 < rpm <= cal_9a, so cal_9a > 0; + * but cal_9a == cal_98 with rpm strictly between them is + * impossible — yet the guard costs nothing). */ + goto reset_path; + } + rw1c = (int16_t)(uint16_t)(rl1c / denom); + /* remainder in rw1e is unused downstream */ + } + +store_011a: + /* 0x5d3e–0x5d43: ST RW1C, *(0x011a) ; SJMP LAB_5d51 */ + rt->tein_overtemp_guard = rw1c; + return; + +reset_path: + /* 0x5d45–0x5d50: ST ZR, *(0x011a) + * ANDB RFE, #0x7F (peripheral clear — not modelled) + * AND RWEA, #0xFFDF (peripheral clear — not modelled) */ + rt->tein_overtemp_guard = 0; + /* 0x5d51–0x5d57: epilogue — handled by C */ +} + +/* + * 1:1 translation of FUN_72b0 @ 0x72b0–0x732c (T06211 variant). + * + * Per-tick producer of `rt->accel_comp_gain` (*(0x0164)). Pipeline: + * 1. eval RPM submap (CAL+0x58) + * 2. eval inj_qty_demand submap (CAL+0x60) + * 3. combine_two_submaps_to_word over the (RPM × demand) table at CAL+0x70 + * 4. eval temperature submap (CAL+0x68) + * 5. refine_submap_result over the 1-D table at CAL+0x72 + * 6. signed MUL: rl1c = refined × combined; SHLL by 8; high word → rt->accel_comp_gain + */ +static void compute_accel_comp_gain(runtime_state_t *rt, const calibration_t *cal) +{ + submap_scratch_t scratch_rpm; + submap_scratch_t scratch_demand; + submap_scratch_t scratch_temp; + + /* 0x72b0–0x72c3: eval RPM submap. */ + eval_submap_to_scratch(&cal->desc_accel_rpm, &scratch_rpm); + + /* 0x72c7–0x72da: eval inj_qty_demand submap. */ + eval_submap_to_scratch(&cal->desc_accel_demand, &scratch_demand); + + /* 0x72de–0x72eb: 2-D combine over the (RPM × demand) table at CAL+0x70. */ + int16_t combined = combine_two_submaps_to_word(cal->accel_combine_table, + &scratch_rpm, + &scratch_demand); + + /* 0x72ef: ADDB R1D, *(0x0408). *(0x0408) is unwritten in this ROM + * (no textual writers found by grep; defaults to boot zero). The + * add is therefore a no-op in T06211's runtime. Same caveat as + * T06031 — flag in open-questions if a future pump family populates + * 0x0408. */ + + /* 0x72f4: ST RW1C, *(0x034a) — caches `combined` for the MUL at + * 0x731e. We just keep it in the C local. */ + + /* 0x72f9–0x730c: eval temperature submap. */ + eval_submap_to_scratch(&cal->desc_accel_temp, &scratch_temp); + + /* 0x7310–0x731a: 1-D refine over the 4-word temperature table at CAL+0x72. */ + int16_t refined = refine_submap_result(cal->accel_refine_table, &scratch_temp); + + /* 0x731e–0x7323: signed MUL — FE prefix on `MUL RL1C, *(0x034a)`. */ + int32_t prod = (int32_t)refined * (int32_t)combined; + + /* 0x7324–0x7326: SHLL RL1C, #0x8 — logical shift-left long by 8. */ + uint32_t shifted = ((uint32_t)prod) << 8; + + /* 0x7327–0x732b: ST RW1E, *(0x0164) — store the HIGH word of the + * shifted product. */ + rt->accel_comp_gain = (int16_t)(uint16_t)(shifted >> 16); + + /* 0x732c: RET. */ +} + +/* + * 1:1 translation of FUN_722e @ 0x722e–0x72af (T06211). + * + * Three RWC6-relative submap evals (RPM, demand, angle_dec_cmd) through + * the 3-D trilinear combine → writes rt->angle_accumulator (RW52) and + * rt->scratch_0158 debug mirror. The conditional FUN_72b0 / FUN_732d + * sub-calls inside the ROM body (0x728b–0x72af) are NOT replicated here — + * they are separate phi_service stages (Stage 3 / Stage 4). + */ +static void compute_angle_accumulator_3d(runtime_state_t *rt, const calibration_t *cal) +{ + /* 0x7241–0x7248: PUSH #0x184 / PUSH RW2A (=RWC6+0) / LCALL FUN_6fb8. + * Evaluates the RPM axis descriptor at RWC6+0 into the scratch buffer + * logically mapped to address 0x0184. */ + eval_submap_to_scratch(&cal->desc_rpm, &rt->scratch_rpm); + + /* 0x724d–0x7258: ADD RW2A,#8 (→RWC6+8) / PUSH #0x194 / PUSH RW2A / LCALL FUN_6fb8. + * Evaluates the inj_qty_demand axis descriptor at RWC6+8 into scratch@0x0194. */ + eval_submap_to_scratch(&cal->desc_demand, &rt->scratch_demand); + + /* 0x725d–0x7268: ADD RW2A,#8 (→RWC6+0x10) / PUSH #0x18c / PUSH RW2A / LCALL FUN_6fb8. + * Evaluates the angle_dec_cmd axis descriptor at RWC6+0x10 into scratch@0x018c. */ + eval_submap_to_scratch(&cal->desc_dec_cmd, &rt->scratch_dec_cmd); + + /* 0x726d–0x727e: PUSH #0x18c / PUSH #0x194 / PUSH #0x184 + * ADD RW2A,#0xa (→RWC6+0x1a) / PUSH [RW2A] / LCALL FUN_7092. + * Axis roles: A=inner(X)=rpm, B=middle(Y)=demand, C=outer(Z)=dec_cmd. */ + int16_t result = combine_three_submaps_to_word(cal->data_table_3d, + &rt->scratch_rpm, + &rt->scratch_demand, + &rt->scratch_dec_cmd); + + /* 0x7283–0x7287: ST RW1C, 0x158,TABLE[ZR] — debug mirror at *(0x0158). */ + rt->scratch_0158 = result; + + /* 0x7288–0x728a: LD RW52, RW1C — write angle_accumulator. */ + rt->angle_accumulator = result; + + /* 0x728b–0x72af: conditional accel_comp_gain / accel_comp_offset + * sub-calls — handled by Stages 3 and 4 of phi_service. */ +} + +/* ══════════════════════════════════════════════════════════════════════ + * compute_temp_phi_comp — 1:1 translation of FUN_6b2a @ 0x6b2a (sign-aware + * wrapper) + FUN_6b4e @ 0x6b4e (unsigned 32-bit IIR step). + * + * Sole producer of `rt->rw9e` — the high word of the signed 32-bit + * IIR-filter accumulator `{rt->rw9e:rt->rw9c}`. Low-passes the + * rpm-derived term `rt->rw9a` (produced by compute_temp_comp_factor at + * 0x6AF9). `rt->rw9e` is consumed by the same compute_temp_comp_factor + * at 0x6AFC. + * + * Cadence: in the ROM the IIR step runs from Timer_1khz (1 kHz) gated + * by an R94 prescaler that reloads from DAT_6061 = 0x64 = 100, so the + * step actually fires at 10 Hz. The compact port preserves both layers: + * the host calls `phi_tick_1khz` every 1 ms; the prescaler in + * `rt->temp_phi_comp_r94` gates the IIR step internally. + * + * Coefficients (per-variant calibration, Q16 unsigned fractions): + * - cal->cal_82 — input gain `b` (CAL[RWA4+0x82]). + * - cal->cal_84 — pole `a` (CAL[RWA4+0x84]). + * On this ROM cal_82 + (uint16_t)cal_84 == 0x10000, giving unity DC gain. + * ══════════════════════════════════════════════════════════════════════ */ + +#define TEMP_PHI_COMP_R94_RELOAD ((uint8_t)100) /* DAT_6061 = 0x64 */ + +/* + * 1:1 port of FUN_6b4e @ 0x6b4e. + * + * 0x6b4e: MULU RL1C, RW9C, CAL[0x84] ; RL1C = RW9C *u CAL[0x84] + * 0x6b54: ST RW1E, RW9C ; RW9C := high(RW9C *u CAL[0x84]) + * 0x6b57: MULU RL1C, RW20, CAL[0x84] ; RL1C = RW20 *u CAL[0x84] + * 0x6b5d: ADD RW9C, RW1C ; RW9C += low(RW20 *u CAL[0x84]); sets C + * 0x6b60: ADDC RW1E, ZR ; RW1E += 0 + C + * 0x6b63: LD RW20, RW1E ; RW20 := high(RW20 *u CAL[0x84]) + carry + * 0x6b66: MULU RL1C, RW9A, CAL[0x82] ; RL1C = RW9A *u CAL[0x82] + * 0x6b6c: ADD RW9C, RW1C ; RW9C += low(RW9A *u CAL[0x82]); sets C + * 0x6b6f: ADDC RW20, RW1E ; RW20 += high(RW9A *u CAL[0x82]) + C + * 0x6b72: RET + * + * The trace operates on the {RW20:RW9C} pair where RW20 is the caller's + * working high-word copy (sign-flipped or not by the FUN_6b2a wrapper). + * All multiplies are unsigned (MULU); all carries propagate exactly as + * the ADD/ADDC chain in the assembly. + */ +static void iir_step_unsigned(uint16_t *rw20, + uint16_t *rw9c, + uint16_t rw9a, + uint16_t cal_82, + uint16_t cal_84) +{ + uint16_t rw1c, rw1e; + uint32_t prod; + + /* 0x6b4e + 0x6b54: rw9c becomes the HIGH word of (rw9c * a). */ + prod = (uint32_t)*rw9c * (uint32_t)cal_84; + rw1e = (uint16_t)(prod >> 16); + *rw9c = rw1e; + + /* 0x6b57: prod = rw20 * a; rw1c = low, rw1e = high. */ + prod = (uint32_t)*rw20 * (uint32_t)cal_84; + rw1c = (uint16_t)(prod & 0xFFFFu); + rw1e = (uint16_t)(prod >> 16); + + /* 0x6b5d / 0x6b60: ADD rw9c, rw1c — capture carry, propagate to rw1e. */ + { + uint32_t s = (uint32_t)*rw9c + rw1c; + *rw9c = (uint16_t)(s & 0xFFFFu); + rw1e = (uint16_t)(rw1e + (s >> 16)); + } + + /* 0x6b63: rw20 := rw1e (= high(rw20*a) + carry). */ + *rw20 = rw1e; + + /* 0x6b66: prod = rw9a * b. */ + prod = (uint32_t)rw9a * (uint32_t)cal_82; + rw1c = (uint16_t)(prod & 0xFFFFu); + rw1e = (uint16_t)(prod >> 16); + + /* 0x6b6c / 0x6b6f: ADD rw9c, rw1c — propagate carry to rw20. */ + { + uint32_t s = (uint32_t)*rw9c + rw1c; + *rw9c = (uint16_t)(s & 0xFFFFu); + *rw20 = (uint16_t)(*rw20 + rw1e + (uint16_t)(s >> 16)); + } +} + +/* + * 1:1 port of FUN_6b2a @ 0x6b2a — sign-aware wrapper around the unsigned IIR step. + * + * 0x6b30: LD RW20, RW9E + * 0x6b33: JBC R21, 0x7, 0x6b42 ; bit 7 of R21 = sign bit of RW20 (= RW9E) + * 0x6b36: NEG RW9C ; RW9C := -RW9C (two's complement) + * 0x6b38: NOT RW20 ; RW20 := ~RW9E (one's complement) + * 0x6b3a: SCALL FUN_6b4e + * 0x6b3c: NOT RW20 ; un-negate + * 0x6b3e: NEG RW9C ; un-negate + * 0x6b40: SJMP 0x6b44 + * 0x6b42: SCALL FUN_6b4e ; RW9E >= 0 path: run IIR step directly + * 0x6b44: LD RW9E, RW20 ; commit new high word + * + * The pre/post (NEG, NOT) pair is two's-complement negation of the + * {RW9E:RW9C} pair when RW9C != 0, and ~RW9E (off-by-one) when RW9C == 0. + * That asymmetry is preserved verbatim — it's the ROM's actual behaviour. + */ +void phi_tick_1khz(phi_state_t *state, const phi_cal_t *cal) +{ + runtime_state_t *rt = &state->rt; + + /* Mirrors the Timer_1khz prescaler at 0x79e2: + * R94 = R94 - 1; if (R94 == 0) { run; R94 = DAT_6061; } */ + if (--rt->temp_phi_comp_r94 != 0u) { + return; + } + rt->temp_phi_comp_r94 = TEMP_PHI_COMP_R94_RELOAD; + + /* 0x6b30: RW20 = RW9E. */ + uint16_t rw20 = (uint16_t)rt->rw9e; + uint16_t rw9c = (uint16_t)rt->rw9c; + uint16_t rw9a = (uint16_t)rt->rw9a; + uint16_t cal_82 = (uint16_t)cal->cal_82; + uint16_t cal_84 = (uint16_t)cal->cal_84; + + if ((int16_t)rt->rw9e < 0) { + /* 0x6b36 / 0x6b38: NEG RW9C, NOT RW20. */ + rw9c = (uint16_t)(-(int16_t)rw9c); + rw20 = (uint16_t)~rw20; + + iir_step_unsigned(&rw20, &rw9c, rw9a, cal_82, cal_84); + + /* 0x6b3c / 0x6b3e: NOT RW20, NEG RW9C. */ + rw20 = (uint16_t)~rw20; + rw9c = (uint16_t)(-(int16_t)rw9c); + } else { + /* 0x6b42: direct call when RW9E sign bit clear. */ + iir_step_unsigned(&rw20, &rw9c, rw9a, cal_82, cal_84); + } + + /* 0x6b44: commit. */ + rt->rw9e = (int16_t)rw20; + rt->rw9c = (int16_t)rw9c; +} + +/* + * 1:1 translation of orphan calc_temp_comp_factor @ 0x6AE7–0x6B29 + * (T06215 variant; algorithmically byte-equivalent to T06211 FUN_5DAB + * @ 0x5DAB). + * + * Per-tick producer of `rt->temp_comp_factor` (the runtime mirror of + * *(0x02FC) in T06215; T06211 uses *(0x02F4)). Reached on-tick by + * `LCALL calc_temp_comp_factor` at 0x7B14 inside the per-cylinder gate + * that precedes the orphan scheduler at FUN_7b1c. + * + * Both MULs in this function carry the FE prefix → signed 16×16→32. + * The final store at 0x6B1E is `ST RW1C, temp_comp` — the LOW word of + * the second product, NOT the high word. + * + * The R0CB-gated `ADD RW20, RWCE` branch at 0x6B00–0x6B07 is + * INTENTIONALLY DROPPED — both register-bank slots are observed zero in + * nominal operation, so the branch is dead. Restore it if a future + * live-ECU dump shows R0CB transitioning out of zero. + * + * See variants/T06211/docs/open-questions.md §9. + */ +static void compute_temp_comp_factor(runtime_state_t *rt, const calibration_t *cal) +{ + /* 0x6AE7–0x6AEC: prologue (PUSH RW1C, RW1E, RW20) — handled by C locals. */ + + /* 0x6AED–0x6AEF: LD RW1C, RW40 — rw1c = rpm. */ + int16_t rpm_signed = (int16_t)rt->rpm; + + /* 0x6AF0–0x6AF5: MUL RL1C, RW1C, temp_comp_dynamic — signed (FE prefix); + * cal_temp_comp_switch_dynamic mirrors temp_comp_dynamic. + * 0x6AF6–0x6AF8: SHRAL RL1C, #2 — arithmetic right shift on the + * 32-bit signed product. */ + int32_t rl1c = (int32_t)rpm_signed + * (int32_t)cal->cal_temp_comp_switch_dynamic; + rl1c = rl1c >> 2; /* arithmetic */ + int16_t rw1e = (int16_t)((rl1c >> 16) & 0xFFFF); /* high word */ + + /* 0x6AF9–0x6AFB: LD RW9A, RW1E — publish high word as state. + * Consumed by other orphans outside the RW48 chain. */ + rt->rw9a = rw1e; + + /* 0x6AFC–0x6AFF: SUB RW20, RW9E, RW1E (3-op) — rw20 = RW9E − rw1e. */ + int16_t rw20 = (int16_t)(rt->rw9e - rw1e); + + /* 0x6B00–0x6B07: R0CB-gated `ADD RW20, RWCE` — DROPPED (see docstring). */ + + /* 0x6B08–0x6B0C: ADD RW20, *(0x0146) — fold in temperature. */ + rw20 = (int16_t)(rw20 + rt->temperature); + + /* 0x6B0D–0x6B11: SUB RW20, [RWA4+0x007E] — temperature reference. */ + rw20 = (int16_t)(rw20 - cal->cal_7e); + + /* 0x6B12–0x6B16: SUB RW20, *(0x0402) — sign-extended boot byte + * (cal_byte_402 = 0xFFF2 = -14 in this T06215 image). */ + rw20 = (int16_t)(rw20 - cal->cal_byte_402); + + /* 0x6B17–0x6B1D: MUL RL1C, RW20, temp_comp_complete — signed (FE prefix); + * cal_temp_comp_switch_complete mirrors temp_comp_complete. + * With switch == 1, rl1c == rw20 (sign-extended). */ + int32_t prod = (int32_t)rw20 + * (int32_t)cal->cal_temp_comp_switch_complete; + + /* 0x6B1E–0x6B22: ST RW1C, temp_comp — LOW word of RL1C. + * NOTE: stores the LOW 16 bits of the signed 32-bit product, not + * the high word. Anything that overflows int16 wraps. */ + rt->temp_comp_factor = (int16_t)((uint32_t)prod & 0xFFFFu); + + /* 0x6B23–0x6B29: epilogue. */ +} + +/* + * 1:1 translation of orphan compute_angle_kick_2d @ 0x6A94–0x6AE6 + * (T06215 variant; algorithmically byte-equivalent to T06211 FUN_5D58 + * @ 0x5D58). + * + * Per-tick producer of `rt->angle_kick_2d` (RW3E). Reached by + * `LCALL compute_angle_kick_2d` at 0x7B76 in the orphan scheduler + * FUN_7b1c, immediately after the angle-accumulator-3D analog and + * immediately before `RW52 += RW3E` at 0x7B79. Reuses scratch_rpm / + * scratch_demand already populated by compute_angle_accumulator_3d + * earlier in the same tick. + * + * Both MULs are SIGNED (FE prefix). SHRAL is arithmetic; SHLL is + * logical. + */ +static void compute_angle_kick_2d(runtime_state_t *rt, const calibration_t *cal) +{ + /* 0x6A94–0x6AA3: prologue (PUSH RW1C/.../RW2A) — handled by C locals. + * 0x6AA4–0x6AAA: RW2A = RWC6 + 0x32 — points at the kick table cal slot. */ + + /* 0x6AAB–0x6AB6: PUSH #0x194 / #0x184 / [RW2A] / LCALL FUN_6f1e + * (T06215's combine_two_submaps_to_word analog). + * 2-D bilinear combine on (rpm, demand) over data_table_2d_kick. + * 0x6AB6: ST RW1C, RW3E — RW3E = bilinear result. */ + int16_t bilinear = combine_two_submaps_to_word(cal->data_table_2d_kick, + &rt->scratch_rpm, + &rt->scratch_demand); + rt->angle_kick_2d = bilinear; + + /* 0x6ABD–0x6AC2: MUL RL1C, RW42, [RWC6+0x34] — signed (FE prefix); + * 0x6AC3–0x6AC5: SHRAL RL1C, #0x8 (arithmetic). + * 0x6AC6–0x6AC8: ADD RW3E, RW1C — fold demand-weighted offset in. */ + int32_t dem_prod = (int32_t)rt->angle_dec_cmd * (int32_t)cal->cal_rwc6_34; + int32_t dem_shifted = dem_prod >> 8; /* arithmetic */ + int16_t dem_low = (int16_t)(dem_shifted & 0xFFFF); + rt->angle_kick_2d = (int16_t)(rt->angle_kick_2d + dem_low); + + /* 0x6AC9–0x6ACF: MUL RL1C, RW3E, temp_comp — signed (FE prefix); + * 0x6AD0–0x6AD2: SHLL RL1C, #0x2 (LOGICAL); take HIGH word. + * 0x6AD3–0x6AD5: ST RW1E, RW3E — RW3E = high_word((RW3E × temp_comp_factor) << 2). */ + int32_t scale_prod = (int32_t)rt->angle_kick_2d * (int32_t)rt->temp_comp_factor; + uint32_t scale_shifted = ((uint32_t)scale_prod) << 2; /* logical */ + rt->angle_kick_2d = (int16_t)(uint16_t)(scale_shifted >> 16); + + /* 0x6AD6–0x6AE6: epilogue. */ +} + +/* + * 1:1 translation of FUN_732d @ 0x732d–0x736d (T06211 variant). + * + * Produces RW3C (rt->accel_comp_offset). MUL at 0x7337 carries the FE + * prefix → SIGNED 16×16→32. SHLL at 0x733d is a logical shift-left + * by 4 on the 32-bit product — the effective result lands in the HIGH + * word (RW1E). Compared to T06031's FUN_5ecf the only differences are + * the calibration offsets used by the clamp: cal_78/cal_7a here vs. + * cal_7e/cal_80 in T06031. + */ +static void compute_accel_comp_offset(runtime_state_t *rt, const calibration_t *cal) +{ + /* 0x732d–0x7330: prologue (PUSH RW1C, RW1E) — handled by C locals */ + + /* 0x7331–0x7336: SUB RW1C, RW40, *(0x0138) (3-operand) + * rw1c = rpm − rpm_baseline. */ + int16_t rw1c = (int16_t)((int16_t)rt->rpm - rt->rpm_baseline); + + /* 0x7337–0x733c: MUL RL1C, *(0x0164) (FE prefix → SIGNED 16×16→32) + * RL1C = rw1c × accel_comp_gain. */ + int32_t rl1c = (int32_t)rw1c * (int32_t)rt->accel_comp_gain; + + /* 0x733d–0x733f: SHLL RL1C, #0x4 + * Logical long shift-left by 4. Effective output is the HIGH word + * (RW1E) — that is what the clamp tests and what gets written. */ + uint32_t u_rl1c = ((uint32_t)rl1c) << 4; + int16_t rw1e = (int16_t)(uint16_t)(u_rl1c >> 16); + + /* 0x7340–0x7359: clamp against CAL[0x78] (upper) and CAL[0x7a] (lower). + * JLE / JGE are SIGNED. cal_78/cal_7a are signed 16-bit values. */ + if (rw1e > cal->cal_78) { + rw1e = cal->cal_78; + } else if (rw1e < cal->cal_7a) { + rw1e = cal->cal_7a; + } + + /* 0x735a–0x7362: enable gate on *(0x0226). + * CMPB ZRlo, *(0x0226); JNE LAB_7363 — fall-through (CLR RW1E) only + * when the byte is zero. */ + if (rt->reset_gate_0226 == 0u) { + rw1e = 0; + } + + /* 0x7363–0x7365: ST RW1E, RW3C — write accel_comp_offset. */ + rt->accel_comp_offset = rw1e; + + /* 0x7366–0x7368: ORB REC, #0x01 — set reset flag so the + * accumulator-inject block in FUN_7453 folds RW3C next tick. */ + rt->rec = (uint8_t)(rt->rec | 0x01u); + + /* 0x7369–0x736d: epilogue (POP RW1E, RW1C, RET) — handled by C */ + + (void)rw1c; /* rw1c is discarded after MUL; preserved for block-mapping */ +} + +/* + * 1:1 translation of the RPM-hysteresis block at 0x7801–0x7825 inside + * the orphan scheduler at 0x77FF (T06211 variant). Bytes were not + * auto-analyzed by Ghidra — verified via + * `python tools/mcs96_disasm.py 0x77F0 100 --variant T06211`. + * + * Updates `rt->gate_0220`: + * rpm < cal_74 → gate_0220 = 0 (deassert) + * rpm >= cal_74 + cal_76 → gate_0220 = 1 (assert) + * in between → gate_0220 unchanged (deadband) + */ +static void compute_gate_0220(runtime_state_t *rt, const calibration_t *cal) +{ + /* 0x7801–0x7806: unsigned compare rpm < cal_74. */ + if ((uint16_t)rt->rpm < (uint16_t)cal->cal_74) { + rt->gate_0220 = 0u; + return; + } + + /* 0x7808–0x7815: unsigned compare rpm < cal_74 + cal_76. */ + uint16_t upper = (uint16_t)((uint16_t)cal->cal_74 + (uint16_t)cal->cal_76); + if ((uint16_t)rt->rpm < upper) { + /* Deadband — leave rt->gate_0220 unchanged. */ + return; + } + + /* 0x7817–0x781A. */ + rt->gate_0220 = 1u; +} + +/* + * 1:1 translation of FUN_62a2 @ 0x62a2–0x6318 (T06211 variant). + * + * Per-tick state machine producing RW7A (rt->tein_valve_fault_guard). + * Two phases driven by a byte-state machine: + * Phase 1 (scratch_010f == 0): RW7A unconditionally cleared, counter + * scratch_010e ramps under slow timing (RWC2 <= dat_604c) and + * decays under fast timing. On firing (counter ≥ scratch_0103), + * RW7A = cal_48 and we advance to Phase 2. + * Phase 2 (scratch_010f != 0): RW7A persists; state byte decays + * under fast timing or re-loads from scratch_0108 under slow + * timing. + * + * Peripheral writes (OR RWEA, #0x200 at 0x62cd) are NO-OPS in the C + * translation — diagnostics fault channel only. + */ +static void compute_tein_valve_fault_guard(runtime_state_t *rt, const calibration_t *cal) +{ + /* 0x62a2–0x62a3: prologue (PUSH RW1C) — handled by C locals */ + + /* 0x62a4–0x62a8: CMPB RF5, #0x0 ; JE LAB_6316 + * Early-out if RF5 (peripheral input gate) is zero. */ + if (rt->rf5_gate == 0u) { + return; + } + + /* 0x62a9–0x62af: CMPB ZRlo, *(0x0221) ; JE LAB_6316 + * Early-out if *(0x0221) (peripheral output flag set by FUN_5a97) + * is zero. */ + if (rt->scratch_0221 == 0u) { + return; + } + + /* 0x62b0–0x62b6: CMPB ZRlo, *(0x010f) ; JNE LAB_62fc + * Branch on Phase-2 state byte. JNE jumps if *(0x010f) != 0 → Phase 2. + * Fall through (==0) → Phase 1. */ + if (rt->scratch_010f != 0u) { + goto phase_2; + } + + /* ──────────────────────────────────────────────────────────────── + * Phase 1 (state == 0): arming / counter ramp / firing + * ──────────────────────────────────────────────────────────────── + */ + + /* 0x62b7–0x62bb: LDB R1C, *(0x010e) — load Phase-1 counter into r1c. */ + uint8_t r1c = rt->scratch_010e; + + /* 0x62bc–0x62be: ST ZR, RW7A — RW7A unconditionally cleared in Phase 1. */ + rt->tein_valve_fault_guard = 0; + + /* 0x62bf–0x62c5: CMP RWC2, dat_604c ; JH LAB_62ee (UNSIGNED). + * JH = unsigned >. If RWC2 > dat_604c → Phase-1 decay branch + * (LAB_62ee). Otherwise fall through to the count-up branch. */ + if (rt->rwc2 > (uint16_t)cal->dat_604c) { + goto phase_1_decay; + } + + /* 0x62c6–0x62cc: ADDB R1C, CAL_byte[RWA4]+0x9c ; INCB R1C + * r1c = scratch_010e + cal_byte_9c + 1, with byte wrap-around. */ + r1c = (uint8_t)(r1c + cal->cal_byte_9c); + r1c = (uint8_t)(r1c + 1u); + + /* 0x62cd–0x62d0: OR RWEA, #0x200 + * Peripheral status bit 9 set — fault/diag channel, not modelled. */ + + /* 0x62d1–0x62d7: CMPB R1C, *(0x0103) ; JNC LAB_62ec (UNSIGNED). + * JNC = unsigned <. If r1c < scratch_0103 → counter not yet at + * threshold, persist r1c and exit. Otherwise fall through to fire. */ + if (r1c >= rt->scratch_0103) { + /* 0x62d8–0x62dc: LD RW1C, CAL[RWA4]+0x48 + * 0x62dd–0x62df: ST RW1C, RW7A — THE NON-ZERO WRITE. */ + rt->tein_valve_fault_guard = cal->cal_48; + + /* 0x62e0–0x62e4: LDB R1C, *(0x0108) — load Phase-2 init value. */ + r1c = rt->scratch_0108; + + /* 0x62e5–0x62e9: STB R1C, *(0x010f) — advance to Phase 2. */ + rt->scratch_010f = r1c; + + /* 0x62ea–0x62eb: CLRB R1C — counter reset (note: r1c here is + * the LOCAL working byte, not scratch_010e; the persisted + * counter is set to 0 via the STB at 0x62f5 which uses this r1c). */ + r1c = 0u; + } + + /* 0x62ec–0x62ed: SJMP LAB_62f5 — fall through to counter store. */ + goto phase_1_store_counter; + +phase_1_decay: + /* 0x62ee–0x62f4: CMPB ZRlo, R1C ; JE LAB_62f5 ; DECB R1C + * Decay branch: if r1c (= scratch_010e on entry) is nonzero, + * decrement it. Saturated at zero. */ + if (r1c != 0u) { + r1c = (uint8_t)(r1c - 1u); + } + +phase_1_store_counter: + /* 0x62f5–0x62f9: STB R1C, *(0x010e) — persist the (possibly + * incremented or decremented) counter value. */ + rt->scratch_010e = r1c; + + /* 0x62fa–0x62fb: SJMP LAB_6316 — exit. */ + return; + +phase_2: + /* ──────────────────────────────────────────────────────────────── + * Phase 2 (state != 0): RW7A holds; state decays / re-loads + * ──────────────────────────────────────────────────────────────── + */ + + { + /* 0x62fc–0x6300: LDB R1C, *(0x010f) — load Phase-2 state. */ + uint8_t r1c2 = rt->scratch_010f; + + /* 0x6301–0x6307: CMP RWC2, dat_604c ; JNH LAB_630c (UNSIGNED). + * JNH = unsigned <=. If RWC2 <= dat_604c → re-load to max. + * Otherwise (RWC2 > dat_604c) → decrement state. */ + if (rt->rwc2 > (uint16_t)cal->dat_604c) { + /* 0x6308–0x6309: DECB R1C — state decays under fast timing. */ + r1c2 = (uint8_t)(r1c2 - 1u); + } else { + /* 0x630c–0x6310: LDB R1C, *(0x0108) — re-load to max under + * slow timing. */ + r1c2 = rt->scratch_0108; + } + + /* 0x6311–0x6315: STB R1C, *(0x010f) — persist new state. */ + rt->scratch_010f = r1c2; + } + + /* 0x6316–0x6318: epilogue (POP RW1C ; RET) — handled by C */ +} + +/* + * Translation of FUN_7453 @ 0x7453–0x74f9 (T06211 variant), scoped to + * RW48 production. Sole writer of target_inj_angle (RW48) at 0x749f + * and target_eoi (RW5A) at 0x74a2. + * + * The upper clamp at 0x7493 is one-sided — no lower clamp — so + * negative results propagate. This matches the assembly exactly and + * must not be "fixed" by adding a lower clamp. + * + * The ROM common-tail (0x74bc–0x74f4 — RW54/RW56/R8B + FUN_739f / + * FUN_5a97 sub-call gate) is omitted: it computes an EPA-scheduling + * lead correction from RW6E (the reluctor tooth-period measurement) + * that never feeds RW48. + */ +static void compute_target_injection_angle(runtime_state_t *rt, const calibration_t *cal) +{ + /* 0x7453–0x7456: prologue (PUSH RW1C, RW1E) — handled by C locals */ + int16_t rw1c; + int16_t rw1e; + + /* 0x7457–0x745a: CMP ZR, RW44 / JE LAB_74b6 + * Early-out: if inj_qty_demand == 0, skip the main computation. + * target_inj_angle and target_eoi are NOT updated on this path — + * they retain the previous tick's value. */ + if (rt->inj_qty_demand == 0) { + return; + } + + /* 0x745c–0x7463: JBC REC, 0x0, LAB_7465 + * If REC.0 (reset_flag) is set, accumulate accel_comp_offset into + * angle_accumulator and clear the ENTIRE REC byte (STB ZRlo, REC). + * T06031's analog at 0x8aad only cleared bit 0 via CLRB — T06211 + * zeros the whole byte here. Preserve that. */ + if ((rt->rec & 0x01u) != 0u) { + rt->angle_accumulator = (int16_t)(rt->angle_accumulator + rt->accel_comp_offset); + rt->rec = 0u; + } + + /* 0x7465–0x7474: CMPB ZRlo, DAT_0200 / JE / ADD RW52, DAT_0200 / ST ZR, DAT_0200 + * Event-accumulator drain: if LOW BYTE of scratch_0200 is nonzero, + * fold the whole WORD into angle_accumulator and clear. The CMPB + * tests ONLY the low byte — a word with just the high byte set + * would skip (matches T06031's pattern). */ + if ((uint8_t)((uint16_t)rt->scratch_0200 & 0xFFu) != 0u) { + rt->angle_accumulator = (int16_t)(rt->angle_accumulator + rt->scratch_0200); + rt->scratch_0200 = 0; + } + + /* 0x7476–0x747a: JBC R53, 0x7, LAB_747b / CLR RW52 + * Saturate angle_accumulator to zero when negative. Assembly tests + * bit 7 of R53 (high byte of RW52) — the sign bit of the signed + * word. If set (value is negative), clear the whole word. */ + if (rt->angle_accumulator < 0) { + rt->angle_accumulator = 0; + } + + /* 0x747b–0x7480: ADD RW1C, RW7A, CAL[RWA4]+0x1e + * 3-operand ADD: rw1c = tein_valve_fault_guard + tein_nominal. */ + rw1c = (int16_t)(rt->tein_valve_fault_guard + cal->tein_nominal); + + /* 0x7481–0x7485: ADD RW1C, 0x11a (2-op) + * rw1c += tein_overtemp_guard (produced by FUN_5ca1 @ 0x5d3e). */ + rw1c = (int16_t)(rw1c + rt->tein_overtemp_guard); + + /* 0x7486–0x7488: MULU RL1C, RW40 — UNSIGNED 16x16 → 32 + * 0x7489–0x748b: SHLL RL1C, #0x1 — logical long shift-left by 1 + * After these two instructions RL1C = (rw1c × rpm) << 1. The low + * word (rw1c) is discarded by the next 3-op ADD at 0x748c; what + * matters is the HIGH word, which lands in RW1E and is subtracted + * from the new rw1c at 0x7490. */ + { + uint32_t rl1c = (uint32_t)(uint16_t)rw1c * (uint32_t)rt->rpm; + rl1c = rl1c << 1; + rw1e = (int16_t)(uint16_t)(rl1c >> 16); + /* low word of rl1c is overwritten by the next ADD */ + } + + /* 0x748c–0x748f: ADD RW1C, RW52, RW42 (3-op) + * rw1c = angle_accumulator + angle_dec_cmd. */ + rw1c = (int16_t)(rt->angle_accumulator + rt->angle_dec_cmd); + + /* 0x7490–0x7492: SUB RW1C, RW1E (2-op) + * Subtract the rpm-scaled fractional term. */ + rw1c = (int16_t)(rw1c - rw1e); + + /* 0x7493–0x749e: CMP RW1C, CAL[RWA4]+0x54 / JLE LAB_749f / LD RW1C, CAL + * Upper-clamp only. JLE is SIGNED. No lower clamp — negative + * results pass through, carrying the "target invalid" sign bit. */ + if (rw1c > cal->cal_54) { + rw1c = cal->cal_54; + } + + /* 0x749f–0x74a1: LD RW48, RW1C — THE WRITE (sole writer of RW48). */ + rt->target_inj_angle = rw1c; + + /* 0x74a2–0x74a7: ADD RW5A, RW1C, 0x14e (3-op, TABLE[ZR]) + * target_eoi = target_inj_angle + phi1, where phi1 is the precomputed + * cell at *(0x014e). The C model recomputes phi1 = phi0 + dphi inline + * (see variants/T06215/docs/open-questions.md §6). */ + { + int16_t phi1 = (int16_t)(cal->phi0 + rt->dphi); + rt->target_eoi = (int16_t)(rw1c + phi1); + } + + /* 0x74a8–0x74f4 (common-tail RW54/RW56/R8B + FUN_739f/FUN_5a97 + * sub-call gate) is scoped out — see function docstring. */ + + /* 0x74f5–0x74f9: epilogue (POP RW1E, RW1C, RET) — handled by C */ +} + +/* ─── Public lifecycle ──────────────────────────────────────────── */ + +void phi_init(phi_state_t *state, const phi_cal_t *cal, + const phi_input_getters_t *getters) +{ + memset(state, 0, sizeof(*state)); + state->getters = getters; + + /* Bind cal descriptors. Cast away const because submap_descriptor_t's + * input_var field is runtime-bound by design (the C model treats the + * cal as a writable buffer, not flash-resident, since extract_*.py + * emits a non-const initializer for this very reason). */ + phi_t06215_bind_inputs(&state->rt, (phi_cal_t *)cal); + + /* Pull boot-only getters — Phase-1 / Phase-2 thresholds for the + * FUN_62a2 state machine. */ + state->rt.scratch_0103 = getters->get_scratch_0103(); + state->rt.scratch_0108 = getters->get_scratch_0108(); + + /* Per-cylinder dispatch bytes r88/r89 and the scratch_01ae table + * only feed the ROM common-tail (RW54/RW56/R8B + FUN_739f/FUN_5a97 + * sub-call gate), which never reaches RW48 — see + * compute_target_injection_angle.c. Scoped out of this port. */ + + /* Seed the IIR prescaler so the 1 kHz hook starts a fresh count from + * boot — mirrors Timer_1khz LDB R94, DAT_6061 at 0x7a26. memset above + * already zeroed rt->rw9c / rt->rw9e (mirrors FUN_6ba3 @ 0x6bbf–0x6bc1). */ + state->rt.temp_phi_comp_r94 = 100u; + + /* Bake scratch_0221 = 1 — FUN_62a2's early-out gate. In the live + * ROM this is written by FUN_5a97 with an RPM-correlated condition + * via RW68 (whose actual writer lives in the unreadable ROM gap + * 0x848e–0x9bbf). Modeling that loop is unnecessary because RW7A + * is monotonic: the only RW48-relevant effect of scratch_0221 is + * "did Phase 1→2 ever latch", and live data shows scratch_0221=1 + * during cranking when that latching occurs. Above ~1400 rpm the + * live ECU flips it to 0 to freeze a counter that's already + * latched — observationally inert. See + * variants/T06211/docs/scratch_0221_writer.md. */ + state->rt.scratch_0221 = 1u; +} + +void phi_service(phi_state_t *state, const phi_cal_t *cal, phi_outputs_t *out) +{ + runtime_state_t *rt = &state->rt; + const phi_input_getters_t *g = state->getters; + + /* ── Stage 1: pull per-tick external inputs ───────────────────── + * These all originate outside the angle-calculation chain. See + * variants/T06211/docs/out-of-scope.md for the off-chain producers + * that drive each one. */ + rt->rpm = g->get_rpm(); + rt->inj_qty_demand = g->get_inj_qty_demand(); + rt->angle_dec_cmd = g->get_angle_dec_cmd(); + rt->temperature = g->get_temperature(); + rt->rpm_baseline = g->get_rpm_baseline(); + rt->rwc2 = g->get_rwc2(); + rt->reset_gate_0226 = g->get_reset_gate_0226(); + rt->dphi = g->get_dphi(); + /* rt->rw9e is produced internally by compute_temp_phi_comp on its + * 10 Hz cadence; it is not pulled from a getter. The host drives the + * 1 kHz hook compute_temp_phi_comp_tick_1khz from a separate timer. */ + + /* Per-tick constants — hard-wire chain inputs that the live ECU drives + * but the simulation pins to a single value: + * rf5_gate = 1 → FUN_62a2 peripheral input gate is permanently + * active in the live ECU; it does not gate the angle + * pipeline in any observable way. */ + rt->rf5_gate = 1u; + + /* Snapshot REC.0 BEFORE FUN_7453 runs — the function clears the + * whole REC byte at 0x7462 if the bit was set, so we have to grab + * it now to expose the "accel_comp_offset folded this tick" flag + * downstream. */ + uint8_t rec0_entering_fun7453; + + /* ── Stage 2: compute_tein_overtemp_guard (FUN_5ca1 @ 0x5ca1) ────────── + * Produces rt->tein_overtemp_guard (the seed addend folded into RW1C inside + * FUN_7453 at 0x7481) from rt->temperature and rt->rpm. The ROM's + * sensor-raw temperature scaler (0x5ca7–0x5cf6) is scoped out — see + * compute_tein_overtemp_guard.c. */ + compute_tein_overtemp_guard(rt, cal); + + /* ── Stage 3: compute_accel_comp_gain (FUN_72b0 @ 0x72b0) ─────── + * 3-submap pipeline (RPM × demand → 2-D combine; temperature → 1-D + * refine; signed MUL high-word → *(0x0164) = rt->accel_comp_gain). + * Always called here; in the ROM the orphan @ 0x77ff gates this on + * a freshly-updated gate_0220, but our simulation runs the producer + * unconditionally so the gain is fresh each tick. */ + compute_accel_comp_gain(rt, cal); + + /* ── Stage 3b: compute_angle_accumulator_3d (FUN_722e @ 0x722e) ──── + * Three RWC6-relative submap evals (RPM, demand, angle_dec_cmd) through + * FUN_7092 (3-D trilinear = combine_three_submaps_to_word) → writes + * rt->angle_accumulator (RW52) and rt->scratch_0158 debug mirror. + * Also populates rt->scratch_rpm and rt->scratch_demand which the + * angle-kick stage (3d) reuses without re-evaluating. The ROM's + * conditional gain/offset sub-calls inside FUN_722e (FUN_72b0 and + * FUN_732d at 0x7292 / 0x729d) are handled by Stage 3 and Stage 4. */ + compute_angle_accumulator_3d(rt, cal); + + /* ── Stage 3c: compute_temp_comp_factor (orphan calc_temp_comp_factor @ 0x6AE7) ─ + * Per-tick rebuild of *(0x02FC) in T06215 (T06211: *(0x02F4)) from rpm, + * temperature, the two boot switches (cal_temp_comp_switch_complete / + * _dynamic), and the external rw9e state input. Mirrors the orphan call + * at 0x7B14 that fires before the angle scheduler at FUN_7b1c. */ + compute_temp_comp_factor(rt, cal); + + /* ── Stage 3d: compute_angle_kick_2d (orphan @ 0x6A94) ───────────── + * 2-D bilinear kick over the table at *(RWC6+0x32), reusing the + * scratches populated by Stage 3b, plus a demand-weighted offset + * `(angle_dec_cmd × cal_rwc6_34) >> 8`, post-scaled by + * temp_comp_factor (Stage 3c output). Mirrors `LCALL compute_angle_kick_2d` + * at 0x7B76. Writes rt->angle_kick_2d (RW3E). */ + compute_angle_kick_2d(rt, cal); + + /* ── Stage 3e: fold the kick into the accumulator + saturate ────── + * Mirrors `ADD RW52, RW3E` at 0x7B79 followed by the `JBC R53.7 / + * CLR RW52` saturate at 0x7B7C–0x7B7F. This happens BEFORE + * compute_target_injection_angle (FUN_7453 analog @ 0x754D); + * FUN_7453's own REC.0 fold of accel_comp_offset (RW3C) and + * *(0x0200) drain run later inside Stage 7. */ + rt->angle_accumulator = (int16_t)(rt->angle_accumulator + rt->angle_kick_2d); + if (rt->angle_accumulator < 0) { + rt->angle_accumulator = 0; + } + + /* ── Stage 4: compute_accel_comp_offset (FUN_732d @ 0x732d) ───── + * Δ-rpm × accel_comp_gain → high-word of (signed MUL << 4), + * clamped against [cal_7a, cal_78]. Forced to 0 when + * reset_gate_0226 == 0, then ORs REC with 0x01 so FUN_7453's reset + * branch folds RW3C into the accumulator next tick. We invoke the + * raw producer (not the gated wrapper FUN_736e) to match the spec — + * the gate_0220 update in stage 5 is informational only. */ + compute_accel_comp_offset(rt, cal); + + /* ── Stage 5: compute_gate_0220 (orphan @ 0x77ff) ─────────────── + * Updates rt->gate_0220 from current rpm with hysteresis bands at + * cal_74 / cal_74+cal_76. Surfaced in the outputs for diagnostic + * tracing; the bare compute_accel_comp_offset above ignores it. */ + compute_gate_0220(rt, cal); + + /* ── Stage 6: compute_tein_valve_fault_guard (FUN_62a2 @ 0x62a2) ─── + * Phase-1/Phase-2 byte-state machine producing RW7A. Uses rwc2 + * vs cal->dat_604c to drive timing. RW7A is added to tein_nominal and + * tein_overtemp_guard inside FUN_7453 at 0x747b/0x7481. */ + compute_tein_valve_fault_guard(rt, cal); + + /* Snapshot REC.0 right before FUN_7453 — used downstream for the + * output flags byte (FUN_7453 clears the whole REC byte at 0x7462 + * if bit 0 was set, so we'd lose this evidence otherwise). */ + rec0_entering_fun7453 = (uint8_t)(rt->rec & 0x01u); + + /* ── Stage 7: compute_target_injection_angle (FUN_7453 @ 0x7453) ── + * Final producer of RW48 (target_inj_angle) and RW5A (target_eoi). + * Runs the inj_qty_demand == 0 alt path internally; on the alt path + * target_inj_angle and target_eoi retain their prior-tick values + * (RW50/activation_angle, the ROM's alt-path source, is not modeled + * — see compute_target_injection_angle.c). The ROM common-tail + * (RW54/RW56/R8B + FUN_739f/FUN_5a97) is scoped out — its DIVU + * uses RW6E (the reluctor tooth period in μs) to produce an + * EPA-scheduling firing-time lead correction that never feeds + * RW48. */ + compute_target_injection_angle(rt, cal); + + /* ── Populate outputs ─────────────────────────────────────────── */ + out->target_inj_angle = rt->target_inj_angle; + out->target_eoi = rt->target_eoi; + out->angle_accumulator = rt->angle_accumulator; + out->accel_comp_offset = rt->accel_comp_offset; + out->accel_comp_gain = rt->accel_comp_gain; + out->tein_valve_fault_guard = rt->tein_valve_fault_guard; + out->tein_overtemp_guard = rt->tein_overtemp_guard; + out->temperature = rt->temperature; + out->temp_comp_factor = rt->temp_comp_factor; + out->angle_kick_2d = rt->angle_kick_2d; + out->gate_0220 = rt->gate_0220; + + out->flags = (uint8_t)(((rt->target_inj_angle < 0) ? 0x01u : 0u) + | ((rec0_entering_fun7453 != 0u) ? 0x02u : 0u) + | ((rt->gate_0220 != 0u) ? 0x04u : 0u)); +} + +size_t phi_state_size(void) { return sizeof(phi_state_t); } +size_t phi_cal_size(void) { return sizeof(phi_cal_t); } diff --git a/Core/Phi/phi.h b/Core/Phi/phi.h new file mode 100644 index 0000000..828fecc --- /dev/null +++ b/Core/Phi/phi.h @@ -0,0 +1,263 @@ +/** + * @file phi.h + * @brief T06215 injection-angle algorithm — compact public interface. + * + * AUTO-GENERATED by tools/extract_t06215_cal.py + * Source ROM: rom_eeprom_dump_0000-9FFF_424026.bin + * Bases: RWA4 = 0x9BD8, RWC6 = 0x7E56 + * + * DO NOT EDIT -- regenerate with: + * python tools/extract_t06215_cal.py --target compact + * + * Companion to phi.c (single-translation-unit port of the per-function + * verbose tree under variants/T06215/src/) and the auto-generated + * phi_cal_tables.c. Callers see only this header; embedded + * `runtime_state_t` / `calibration_t` fields are IMPLEMENTATION DETAIL + * — touch only the outputs struct and the getter vtable. + * + * Lifecycle: + * + * phi_input_getters_t getters = { .get_rpm = ..., ... }; + * phi_state_t state; + * phi_cal_t cal = phi_t06215_cal; // copy from auto-extracted master + * phi_outputs_t out; + * + * phi_init(&state, &cal, &getters); // boot-once + * for each tick: + * phi_service(&state, &cal, &out); + * use(out.target_inj_angle, ...); + * + * The producer chain runs in this fixed order each tick (see + * variants/T06211/docs/open-questions.md §4 / §5 — fused scheduler + * `FUN_698C` enclosing `0x6a3f LCALL FUN_7453` is authoritative): + * + * 1. CAN / external inputs pulled via the getter vtable. + * 2. compute_tein_overtemp_guard (FUN_5ca1) -> temperature, tein_overtemp_guard + * 3. compute_accel_comp_gain (FUN_72b0) -> accel_comp_gain @ *(0x0164) + * 4. compute_accel_comp_offset (FUN_732d) -> RW3C, REC.0 + * 5. compute_gate_0220 (orphan @ 0x77ff) -> gate_0220 hysteresis + * 6. compute_tein_valve_fault_guard (FUN_62a2) -> RW7A + * 7. compute_target_injection_angle (FUN_7453) -> RW48 / RW5A + * + * The orphan temperature-kick chain (compute_temp_comp_factor → orphan + * FUN_5D58 angle_kick + `RW52 += RW3E` fold-in @ 0x7A3E) is ported in + * variants/T06215/src/ and wired into this compact phi_service. + * RW9E (the IIR state high word consumed by compute_temp_comp_factor) + * is produced internally by compute_temp_phi_comp — see + * compute_temp_phi_comp.h for the 1 kHz hook the host must drive. + * See variants/T06211/docs/open-questions.md §9. + */ +#ifndef PHI_T06215_H +#define PHI_T06215_H + +#include +#include + +/* ─── Internal runtime / calibration types ─────────────────────────── + * Inlined verbatim from variants/T06215/src/injection_angle_state.h. + * Treat all fields as IMPLEMENTATION DETAIL — see the lifecycle + * comment above; the supported surface is `phi_outputs_t` and the + * getter vtable. */ + +typedef struct { + int16_t stride_bytes; /* +0: 2 * stride_items */ + int16_t delta; /* +2: axis[idx-1] - axis[idx] */ + int16_t fraction; /* +4: input_var - axis[idx] */ + int16_t index_bytes; /* +6: byte offset into the axis array */ +} submap_scratch_t; + +typedef struct { + int16_t runtime_slot; /* +0: written by eval_submap_to_scratch */ + int16_t *input_var; /* +2: pointer to the RW input variable */ + int16_t stride_items; /* +4: breakpoints in the axis */ + const int16_t *axis; /* +6: axis breakpoint table (descending) */ +} submap_descriptor_t; + +typedef struct { + /* ---- Per-tick inputs (produced elsewhere / by earlier producers) ---- */ + uint16_t rpm; /* RW40 @ 0x0040 — engine RPM (external, HSI.0 ISR) */ + int16_t angle_dec_cmd; /* RW42 @ 0x0042 — angle-dec command (external input) */ + int16_t inj_qty_demand; /* RW44 @ 0x0044 — injection-quantity command. EXTERNAL CAN input. */ + int16_t accel_comp_offset; /* RW3C @ 0x003C — internally produced by compute_accel_comp_offset (FUN_732d). */ + int16_t tein_valve_fault_guard; /* RW7A @ 0x007A — internally produced by compute_tein_valve_fault_guard (FUN_62a2). */ + + /* ---- Reset/run flag byte (REC) ---- */ + uint8_t rec; /* REC — bit 0 tested by FUN_7453 at 0x745c. */ + + /* ---- Persistent accumulator ---- */ + int16_t angle_accumulator; /* RW52 @ 0x0052 — class-B state, updated in-place by FUN_7453. */ + + /* ---- Per-tick angle-kick fold-in (orphan FUN_5D58 / 0x7A3E) ---- */ + int16_t angle_kick_2d; /* RW3E @ 0x003E — produced by compute_angle_kick_2d (port of orphan + * FUN_5D58 @ 0x5D58). Folded into angle_accumulator at 0x7A3E + * before FUN_7453. */ + + /* ---- Outputs of the chain ---- */ + int16_t target_inj_angle; /* RW48 @ 0x0048 — SOLE writer is FUN_7453 @ 0x749f */ + int16_t target_eoi; /* RW5A @ 0x005A — written @ 0x74a2 as target_inj_angle + phi1. */ + + /* ---- Absolute-address RAM touched by FUN_7453 ---- */ + int16_t scratch_0200; /* *(0x0200) — event accumulator. */ + int16_t tein_overtemp_guard; /* *(0x011a) — seed addend, produced by FUN_5ca1 @ 0x5d3e. */ + int16_t dphi; /* *(0x014c) — angle delta. */ + + /* ---- Inputs to FUN_732d (compute_accel_comp_offset) ---- */ + int16_t rpm_baseline; /* *(0x0138) — slow RPM baseline. */ + int16_t accel_comp_gain; /* *(0x0164) — Δ-rpm × gain multiplier. */ + uint8_t reset_gate_0226; /* *(0x0226) — accel_comp_offset enable gate. */ + uint8_t gate_0220; /* *(0x0220) — RPM-hysteresis gate. */ + + /* ---- Inputs/state for FUN_5ca1 (compute_tein_overtemp_guard) ---- */ + int16_t temperature; /* *(0x0146) — engine temperature (external input boundary). */ + + /* ---- Per-tick temperature-compensation factor (orphan FUN_5DAB / 0x5DE2) ---- + * The R0CB-gated `ADD RW20, RWCE` branch at 0x5DC4–0x5DCB is + * intentionally not modeled — both slots are observed zero. */ + int16_t temp_comp_factor; /* *(0x02F4) — produced by compute_temp_comp_factor (port of orphan + * FUN_5DAB @ 0x5DAB). Consumed by compute_angle_kick_2d at 0x5D8D. */ + int16_t rw9a; /* RW9A @ 0x009A — published by compute_temp_comp_factor at 0x5DBD. */ + int16_t rw9c; /* RW9C @ 0x009C — low word of the {rw9e:rw9c} signed IIR-filter + * state pair updated by compute_temp_phi_comp (port of FUN_6b4e + * @ 0x6b4e). Boot-zero (FUN_6ba3 @ 0x6bbf clears it). */ + int16_t rw9e; /* RW9E @ 0x009E — high word of the {rw9e:rw9c} signed IIR-filter + * state pair, sole writer is phi_tick_1khz at the 10 Hz + * derived rate (port of FUN_6b2a @ 0x6b2a). Consumed by + * compute_temp_comp_factor at 0x6AFC. */ + uint8_t temp_phi_comp_r94; /* R94 byte register slot driven by Timer_1khz at 0x79e2 / 0x7a26. + * Prescaler that gates the IIR step inside phi_tick_1khz: counts + * down on every 1 ms call, IIR fires when it reaches 0 and reloads + * to DAT_6061 = 0x64 = 100 → effective 10 Hz cadence. Seeded to + * 100 by phi_init. */ + + /* ---- 3-D angle-accumulator submap scratches (FUN_722e @ 0x722e) ---- */ + submap_scratch_t scratch_rpm; /* *(0x0184) — RPM axis scratch. */ + submap_scratch_t scratch_demand; /* *(0x0194) — inj_qty_demand axis scratch. */ + submap_scratch_t scratch_dec_cmd; /* *(0x018c) — angle_dec_cmd axis scratch. */ + int16_t scratch_0158; /* *(0x0158) — debug mirror of angle_accumulator result. */ + + /* ---- State for FUN_62a2 (compute_tein_valve_fault_guard, RW7A producer) ---- */ + uint16_t rwc2; /* RWC2 — timing-baseline scratch compared against dat_604c. */ + uint8_t rf5_gate; /* RF5 byte — peripheral input gate. */ + uint8_t scratch_0221; /* *(0x0221) — FUN_62a2 early-out gate (baked to 1 in phi_init). */ + uint8_t scratch_010e; /* *(0x010e) — Phase-1 counter byte. */ + uint8_t scratch_010f; /* *(0x010f) — Phase-2 state byte. */ + uint8_t scratch_0103; /* *(0x0103) — Phase-1 counter firing threshold. */ + uint8_t scratch_0108; /* *(0x0108) — Phase-2 init/re-load value. */ +} runtime_state_t; + +typedef struct { + /* ---- RWA4-relative scalar calibration ---- */ + int16_t tein_nominal; /* CAL[0x1e] — seed base added to tein_valve_fault_guard at 0x747b. */ + int16_t cal_48; /* CAL[0x48] — value loaded into RW7A when FUN_62a2 fires (= 0x04B0). */ + int16_t phi0; /* CAL[0x4c] — base/initial angle. */ + uint8_t cal_byte_9c; /* CAL_byte[0x9c] — counter increment in FUN_62a2 (= 0x03). */ + int16_t cal_54; /* CAL[0x54] — UPPER clamp on target_inj_angle at 0x7493. */ + int16_t cal_74; /* CAL[0x74] — LOWER RPM threshold for gate_0220 hysteresis. */ + int16_t cal_76; /* CAL[0x76] — gate_0220 hysteresis width. */ + int16_t cal_78; /* CAL[0x78] — UPPER clamp on accel_comp_offset. */ + int16_t cal_7a; /* CAL[0x7a] — LOWER clamp on accel_comp_offset. */ + int16_t cal_7e; /* CAL[0x7e] — temperature reference subtrahend in compute_temp_comp_factor (FUN_5DAB 0x5DD1). */ + int16_t cal_temp_comp_switch_dynamic; /* CAL[0x80] — boot value of *(0x02F6) (German label TK_AT_W). */ + int16_t cal_82; /* CAL[0x82] — IIR input-gain `b` (Q16 unsigned) used by compute_temp_phi_comp + * at the FUN_6b4e MULU @ 0x6B66. Read via (uint16_t) cast. */ + int16_t cal_84; /* CAL[0x84] — IIR pole `a` (Q16 unsigned) used by compute_temp_phi_comp + * at the FUN_6b4e MULUs @ 0x6B4E / 0x6B57. Read via (uint16_t) cast. + * On this ROM cal_82 + (uint16_t)cal_84 == 0x10000 (unity DC). */ + int16_t cal_temp_comp_switch_complete; /* CAL[0x86] — boot value of *(0x02F2) (German label F_TK_TE_W). */ + int16_t cal_92; /* CAL[0x92] — temperature offset subtracted in FUN_5ca1. */ + int16_t cal_94; /* CAL[0x94] — multiplier in FUN_5ca1. */ + int16_t cal_96; /* CAL[0x96] — UPPER clamp on RW1C in FUN_5ca1 (unsigned). */ + int16_t cal_98; /* CAL[0x98] — LOWER rpm threshold for tein_overtemp_guard. */ + int16_t cal_9a; /* CAL[0x9a] — UPPER rpm threshold for tein_overtemp_guard. */ + + /* ---- accel_comp_gain (FUN_72b0) submaps & tables (RWA4-relative) ---- */ + submap_descriptor_t desc_accel_rpm; /* CAL+0x58 — RPM axis */ + submap_descriptor_t desc_accel_demand; /* CAL+0x60 — inj_qty_demand axis */ + submap_descriptor_t desc_accel_temp; /* CAL+0x68 — temperature axis */ + const int16_t *accel_combine_table; /* CAL+0x70 — 2D RPM×demand combine table */ + const int16_t *accel_refine_table; /* CAL+0x72 — 1D temperature refine table */ + + /* ---- ROM constants ---- */ + int16_t dat_604c; /* *(0x604c) = 0x0444 — RWC2 timing threshold in FUN_62a2. */ + int16_t cal_byte_402; /* *(0x0402) — boot sign-extended byte. Subtracted in FUN_5DAB at 0x5DD6. */ + + /* ---- RWC6-relative: 3-D angle-accumulator submap (FUN_722e) ---- */ + submap_descriptor_t desc_rpm; /* RWC6+0x00 — RPM axis */ + submap_descriptor_t desc_demand; /* RWC6+0x08 — inj_qty_demand axis */ + submap_descriptor_t desc_dec_cmd; /* RWC6+0x10 — angle_dec_cmd axis */ + const int16_t *data_table_3d; /* word @ RWC6+0x1a — 3-D table data */ + + /* ---- RWC6-relative: 2-D angle-kick submap + scalar (FUN_5D58) ---- */ + const int16_t *data_table_2d_kick; /* word @ RWC6+0x32 — 2-D kick table (RPM × demand). */ + int16_t cal_rwc6_34; /* CAL[RWC6+0x34] — demand-weight multiplier in FUN_5D58. */ +} calibration_t; + +/* ─── Outputs surfaced after every phi_service ───────────────────── */ +typedef struct { + int16_t target_inj_angle; /* RW48 — primary output (FUN_7453 @ 0x749f) */ + int16_t target_eoi; /* RW5A — target_inj_angle + phi1; phi1 = phi0 + dphi */ + int16_t angle_accumulator; /* RW52 — class-B accumulator state */ + int16_t accel_comp_offset; /* RW3C — FUN_732d output */ + int16_t accel_comp_gain; /* *(0x0164) — FUN_72b0 output */ + int16_t tein_valve_fault_guard; /* RW7A — FUN_62a2 output */ + int16_t tein_overtemp_guard; /* *(0x011a) — FUN_5ca1 seed addend */ + int16_t temperature; /* *(0x0146) — FUN_5ca1 filtered temperature */ + int16_t temp_comp_factor; /* *(0x02F4) — compute_temp_comp_factor (FUN_5DAB) output */ + int16_t angle_kick_2d; /* RW3E — compute_angle_kick_2d (FUN_5D58) output (folded into angle_accumulator) */ + uint8_t gate_0220; /* *(0x0220) — RPM hysteresis gate */ + uint8_t flags; /* bit 0: target_inj_angle invalid (sign bit set); + * bit 1: REC.0 was set entering FUN_7453; + * bit 2: gate_0220 active */ +} phi_outputs_t; + +/* ─── Getter vtable ────────────────────────────────────────────────── */ +typedef struct { + int16_t (*get_inj_qty_demand)(void); /* RW44 */ + int16_t (*get_angle_dec_cmd)(void); /* RW42 */ + uint16_t (*get_rpm)(void); /* RW40 */ + int16_t (*get_temperature)(void); /* *(0x0146) */ + int16_t (*get_rpm_baseline)(void); /* *(0x0138) */ + uint16_t (*get_rwc2)(void); /* RWC2 */ + uint8_t (*get_reset_gate_0226)(void); /* *(0x0226) */ + int16_t (*get_dphi)(void); /* *(0x014c) */ + uint8_t (*get_scratch_0103)(void); /* *(0x0103) — boot-only */ + uint8_t (*get_scratch_0108)(void); /* *(0x0108) — boot-only */ +} phi_input_getters_t; + +/* ─── Lifecycle types ────────────────────────────────────────────────── */ +typedef struct { + runtime_state_t rt; + const phi_input_getters_t *getters; +} phi_state_t; + +typedef calibration_t phi_cal_t; + +/* ─── Public API ───────────────────────────────────────────────────── */ + +void phi_init(phi_state_t *state, + const phi_cal_t *cal, + const phi_input_getters_t *getters); + +void phi_service(phi_state_t *state, + const phi_cal_t *cal, + phi_outputs_t *out); + +/** 1 kHz hook. Host invokes once every 1 ms from its millisecond timer. + * Internally decrements `state->rt.temp_phi_comp_r94`; on R94 == 0 runs + * the IIR step (port of FUN_6b2a + FUN_6b4e @ 0x6b2a / 0x6b4e) and + * reloads R94 to 100 (DAT_6061). The IIR updates `{rt->rw9e, rt->rw9c}` + * in place using `rt->rw9a` as the input and `cal->cal_82` / `cal->cal_84` + * as coefficients. Reentrant per phi_state_t — the prescaler lives in + * runtime_state_t. */ +void phi_tick_1khz(phi_state_t *state, const phi_cal_t *cal); + +size_t phi_state_size(void); +size_t phi_cal_size(void); + +/* ─── Auto-generated cal export (defined in phi_cal_tables.c) ──────── */ + +extern calibration_t phi_t06215_cal; + +void phi_t06215_bind_inputs(runtime_state_t *rt, calibration_t *cal); + +#endif /* PHI_T06215_H */ diff --git a/Core/Phi/phi_cal_tables.c b/Core/Phi/phi_cal_tables.c new file mode 100644 index 0000000..03c6ebf --- /dev/null +++ b/Core/Phi/phi_cal_tables.c @@ -0,0 +1,247 @@ +/** + * @file phi_cal_tables.c + * @brief T06215 compact-port calibration data (auto-generated). + * + * AUTO-GENERATED by tools/extract_t06215_cal.py + * Source ROM: rom_eeprom_dump_0000-9FFF_424026.bin + * Bases: RWA4 = 0x9BD8, RWC6 = 0x7E56 + * + * DO NOT EDIT -- regenerate with: + * python tools/extract_t06215_cal.py --target compact + */ +#include +#include "phi.h" + +/* ====================================================================== + * Accel axes + data tables (file-local; expose only phi_t06215_cal). + * Angle accumulator axes + 3-D table (FUN_722e producer chain). + * ====================================================================== */ + +/* Accel RPM axis -- 7 words @ 0x9D40. */ +static const int16_t phi_accel_axis_rpm[7] = { + (int16_t)0x4B5E, (int16_t)0x3127, (int16_t)0x16F0, (int16_t)0x1206, (int16_t)0x09D5, (int16_t)0x068E, (int16_t)0x0000 +}; + +/* Accel inj_qty_demand axis -- 6 words @ 0x9D4E. */ +static const int16_t phi_accel_axis_demand[6] = { + (int16_t)0x08C0, (int16_t)0x0640, (int16_t)0x0460, (int16_t)0x0280, (int16_t)0x01E0, (int16_t)0x0000 +}; + +/* Accel temperature axis -- 5 words @ 0x9D5A. */ +static const int16_t phi_accel_axis_temp[5] = { + (int16_t)0x12F0, (int16_t)0x1250, (int16_t)0x1110, (int16_t)0x0FD0, (int16_t)0x0000 +}; + +/* Accel 2-D combine (RPM x demand) -- 42 words @ 0x9D64. */ +static const int16_t phi_accel_combine_table[42] = { + (int16_t)0x0000, (int16_t)0x020A, (int16_t)0x052C, (int16_t)0x082A, (int16_t)0x082A, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x020A, (int16_t)0x052C, + (int16_t)0x082A, (int16_t)0x082A, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x020A, (int16_t)0x052C, (int16_t)0x082A, (int16_t)0x082A, (int16_t)0x0000, + (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x020A, (int16_t)0x052C, (int16_t)0x082A, (int16_t)0x082A, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x020A, + (int16_t)0x052C, (int16_t)0x082A, (int16_t)0x082A, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x020A, (int16_t)0x052C, (int16_t)0x082A, (int16_t)0x082A, + (int16_t)0x0000, (int16_t)0x0000 +}; + +/* Accel 1-D refine (temperature) -- 5 words @ 0x9DB8. */ +static const int16_t phi_accel_refine_table[5] = { + (int16_t)0x00FF, (int16_t)0x00FF, (int16_t)0x00FF, (int16_t)0x00FF, (int16_t)0x00FF +}; + +/* Angle RPM axis -- 13 words @ 0x7E8C. */ +static const int16_t phi_angle_axis_rpm[13] = { + (int16_t)0x4B5E, (int16_t)0x4674, (int16_t)0x4189, (int16_t)0x3AFB, (int16_t)0x346E, (int16_t)0x2DE0, (int16_t)0x240B, (int16_t)0x1A37, (int16_t)0x13A9, (int16_t)0x0D1B, + (int16_t)0x068E, (int16_t)0x0347, (int16_t)0x0000 +}; + +/* Angle inj_qty_demand axis -- 15 words @ 0x7EA6. */ +static const int16_t phi_angle_axis_demand[15] = { + (int16_t)0x0F90, (int16_t)0x0C73, (int16_t)0x0A60, (int16_t)0x0956, (int16_t)0x08D2, (int16_t)0x07C8, (int16_t)0x063A, (int16_t)0x04AB, (int16_t)0x031D, (int16_t)0x0213, + (int16_t)0x010A, (int16_t)0x0085, (int16_t)0x0035, (int16_t)0x000D, (int16_t)0x0000 +}; + +/* Angle angle_dec_cmd axis -- 3 words @ 0x7EC4. */ +static const int16_t phi_angle_axis_dec_cmd[3] = { + (int16_t)0x0300, (int16_t)0x01C4, (int16_t)0x0000 +}; + +/* 3-D angle combine (RPM x demand x angle_dec_cmd) -- 585 words @ 0x7ECA. */ +static const int16_t phi_angle_3d_table[585] = { + (int16_t)0x0508, (int16_t)0x07FB, (int16_t)0x0A1A, (int16_t)0x097B, (int16_t)0x08AD, (int16_t)0x07A5, (int16_t)0x0654, (int16_t)0x04FF, (int16_t)0x046F, (int16_t)0x03A0, + (int16_t)0x034F, (int16_t)0x02A7, (int16_t)0x02A7, (int16_t)0x0508, (int16_t)0x07FB, (int16_t)0x0A1A, (int16_t)0x097B, (int16_t)0x08AD, (int16_t)0x07A5, (int16_t)0x0654, + (int16_t)0x04FF, (int16_t)0x046F, (int16_t)0x03A0, (int16_t)0x034F, (int16_t)0x02A7, (int16_t)0x02A7, (int16_t)0x0508, (int16_t)0x07FB, (int16_t)0x0A1A, (int16_t)0x097B, + (int16_t)0x08AD, (int16_t)0x07A5, (int16_t)0x0654, (int16_t)0x04FF, (int16_t)0x046F, (int16_t)0x03A0, (int16_t)0x02F4, (int16_t)0x0238, (int16_t)0x0238, (int16_t)0x0508, + (int16_t)0x07FB, (int16_t)0x0A1A, (int16_t)0x097B, (int16_t)0x08AD, (int16_t)0x07A5, (int16_t)0x0654, (int16_t)0x04FF, (int16_t)0x042A, (int16_t)0x0367, (int16_t)0x02BB, + (int16_t)0x020C, (int16_t)0x020C, (int16_t)0x0508, (int16_t)0x07FB, (int16_t)0x0A1A, (int16_t)0x0956, (int16_t)0x086A, (int16_t)0x0772, (int16_t)0x0620, (int16_t)0x04D4, + (int16_t)0x0407, (int16_t)0x034F, (int16_t)0x02A4, (int16_t)0x01FA, (int16_t)0x01FA, (int16_t)0x0506, (int16_t)0x07F9, (int16_t)0x0974, (int16_t)0x0892, (int16_t)0x07C3, + (int16_t)0x06F0, (int16_t)0x05BA, (int16_t)0x0490, (int16_t)0x03C7, (int16_t)0x031C, (int16_t)0x0271, (int16_t)0x01D4, (int16_t)0x01D4, (int16_t)0x0504, (int16_t)0x07F7, + (int16_t)0x0838, (int16_t)0x0787, (int16_t)0x06D2, (int16_t)0x0628, (int16_t)0x050A, (int16_t)0x0413, (int16_t)0x036D, (int16_t)0x02DA, (int16_t)0x0229, (int16_t)0x019C, + (int16_t)0x019C, (int16_t)0x0501, (int16_t)0x07F4, (int16_t)0x0766, (int16_t)0x06C4, (int16_t)0x0626, (int16_t)0x0576, (int16_t)0x0487, (int16_t)0x039A, (int16_t)0x030F, + (int16_t)0x0291, (int16_t)0x01DE, (int16_t)0x0167, (int16_t)0x0167, (int16_t)0x04FF, (int16_t)0x06F4, (int16_t)0x068B, (int16_t)0x0600, (int16_t)0x057C, (int16_t)0x04EE, + (int16_t)0x03F4, (int16_t)0x031F, (int16_t)0x02A0, (int16_t)0x0231, (int16_t)0x018C, (int16_t)0x0140, (int16_t)0x0140, (int16_t)0x04FD, (int16_t)0x0661, (int16_t)0x05F9, + (int16_t)0x0582, (int16_t)0x04F9, (int16_t)0x0480, (int16_t)0x03AA, (int16_t)0x02D0, (int16_t)0x0261, (int16_t)0x01E6, (int16_t)0x0164, (int16_t)0x0121, (int16_t)0x0121, + (int16_t)0x04FD, (int16_t)0x05FD, (int16_t)0x059B, (int16_t)0x0524, (int16_t)0x04A7, (int16_t)0x041E, (int16_t)0x0363, (int16_t)0x029E, (int16_t)0x020E, (int16_t)0x0190, + (int16_t)0x0136, (int16_t)0x0103, (int16_t)0x0103, (int16_t)0x04FD, (int16_t)0x0593, (int16_t)0x0530, (int16_t)0x04C0, (int16_t)0x045A, (int16_t)0x03EF, (int16_t)0x0325, + (int16_t)0x0260, (int16_t)0x01E7, (int16_t)0x0158, (int16_t)0x011B, (int16_t)0x00AB, (int16_t)0x00AB, (int16_t)0x04FD, (int16_t)0x0523, (int16_t)0x04CB, (int16_t)0x0461, + (int16_t)0x03F5, (int16_t)0x038F, (int16_t)0x02EB, (int16_t)0x022F, (int16_t)0x01B1, (int16_t)0x0138, (int16_t)0x00BB, (int16_t)0x009B, (int16_t)0x009B, (int16_t)0x04FD, + (int16_t)0x04E5, (int16_t)0x0492, (int16_t)0x041D, (int16_t)0x03B0, (int16_t)0x034F, (int16_t)0x029C, (int16_t)0x01F1, (int16_t)0x0178, (int16_t)0x011B, (int16_t)0x00AE, + (int16_t)0x008B, (int16_t)0x008B, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, + (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0648, (int16_t)0x08FE, (int16_t)0x0B09, (int16_t)0x0A71, (int16_t)0x0972, + (int16_t)0x0889, (int16_t)0x0729, (int16_t)0x05D6, (int16_t)0x0552, (int16_t)0x046C, (int16_t)0x0420, (int16_t)0x035C, (int16_t)0x035C, (int16_t)0x0648, (int16_t)0x08FE, + (int16_t)0x0B09, (int16_t)0x0A71, (int16_t)0x0972, (int16_t)0x0889, (int16_t)0x0729, (int16_t)0x05D6, (int16_t)0x0552, (int16_t)0x046C, (int16_t)0x0420, (int16_t)0x035C, + (int16_t)0x035C, (int16_t)0x0648, (int16_t)0x08FE, (int16_t)0x0B09, (int16_t)0x0A71, (int16_t)0x0972, (int16_t)0x0889, (int16_t)0x0729, (int16_t)0x05D6, (int16_t)0x0552, + (int16_t)0x046C, (int16_t)0x03C1, (int16_t)0x0314, (int16_t)0x0314, (int16_t)0x0648, (int16_t)0x08FE, (int16_t)0x0B09, (int16_t)0x0A71, (int16_t)0x0972, (int16_t)0x0889, + (int16_t)0x0729, (int16_t)0x05D6, (int16_t)0x050F, (int16_t)0x043B, (int16_t)0x039A, (int16_t)0x02EE, (int16_t)0x02EE, (int16_t)0x0648, (int16_t)0x08FE, (int16_t)0x0B09, + (int16_t)0x0A19, (int16_t)0x092F, (int16_t)0x084F, (int16_t)0x06F8, (int16_t)0x05AF, (int16_t)0x04ED, (int16_t)0x0425, (int16_t)0x0384, (int16_t)0x02D9, (int16_t)0x02D9, + (int16_t)0x0646, (int16_t)0x08FC, (int16_t)0x0A4D, (int16_t)0x0975, (int16_t)0x089C, (int16_t)0x07D0, (int16_t)0x0697, (int16_t)0x0564, (int16_t)0x04B1, (int16_t)0x03F8, + (int16_t)0x0351, (int16_t)0x02AF, (int16_t)0x02AF, (int16_t)0x0644, (int16_t)0x08FA, (int16_t)0x0939, (int16_t)0x086F, (int16_t)0x07B7, (int16_t)0x0711, (int16_t)0x0602, + (int16_t)0x04FA, (int16_t)0x0458, (int16_t)0x03B0, (int16_t)0x0305, (int16_t)0x0280, (int16_t)0x0280, (int16_t)0x0641, (int16_t)0x08F7, (int16_t)0x0867, (int16_t)0x07B6, + (int16_t)0x0707, (int16_t)0x066B, (int16_t)0x0570, (int16_t)0x0488, (int16_t)0x03FA, (int16_t)0x035E, (int16_t)0x02B1, (int16_t)0x0251, (int16_t)0x0251, (int16_t)0x063F, + (int16_t)0x07FB, (int16_t)0x077B, (int16_t)0x06E4, (int16_t)0x063D, (int16_t)0x05CB, (int16_t)0x04E8, (int16_t)0x040E, (int16_t)0x03A1, (int16_t)0x031B, (int16_t)0x0273, + (int16_t)0x021C, (int16_t)0x021C, (int16_t)0x063D, (int16_t)0x0761, (int16_t)0x06E7, (int16_t)0x064D, (int16_t)0x05BF, (int16_t)0x0552, (int16_t)0x048A, (int16_t)0x03BE, + (int16_t)0x0354, (int16_t)0x02D6, (int16_t)0x0238, (int16_t)0x01F5, (int16_t)0x01F5, (int16_t)0x063D, (int16_t)0x06C1, (int16_t)0x0666, (int16_t)0x05DA, (int16_t)0x0574, + (int16_t)0x04FF, (int16_t)0x0421, (int16_t)0x0361, (int16_t)0x02EA, (int16_t)0x0274, (int16_t)0x0201, (int16_t)0x01D2, (int16_t)0x01D2, (int16_t)0x063D, (int16_t)0x0663, + (int16_t)0x05F9, (int16_t)0x0586, (int16_t)0x0527, (int16_t)0x04B7, (int16_t)0x03E0, (int16_t)0x0316, (int16_t)0x02A2, (int16_t)0x0239, (int16_t)0x01A0, (int16_t)0x016B, + (int16_t)0x016B, (int16_t)0x063D, (int16_t)0x063D, (int16_t)0x05CF, (int16_t)0x0553, (int16_t)0x04FB, (int16_t)0x048E, (int16_t)0x03C6, (int16_t)0x02E9, (int16_t)0x0275, + (int16_t)0x01FB, (int16_t)0x0166, (int16_t)0x012F, (int16_t)0x012F, (int16_t)0x063D, (int16_t)0x0619, (int16_t)0x05B8, (int16_t)0x052C, (int16_t)0x04D3, (int16_t)0x0466, + (int16_t)0x03A0, (int16_t)0x02BC, (int16_t)0x0249, (int16_t)0x01C4, (int16_t)0x013B, (int16_t)0x0104, (int16_t)0x0104, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, + (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, + (int16_t)0x079D, (int16_t)0x0AB3, (int16_t)0x0CC8, (int16_t)0x0C36, (int16_t)0x0B3C, (int16_t)0x0A48, (int16_t)0x09F1, (int16_t)0x0896, (int16_t)0x0796, (int16_t)0x068B, + (int16_t)0x066B, (int16_t)0x067C, (int16_t)0x067C, (int16_t)0x079D, (int16_t)0x0AB3, (int16_t)0x0CC8, (int16_t)0x0C36, (int16_t)0x0B3C, (int16_t)0x0A48, (int16_t)0x09F1, + (int16_t)0x0896, (int16_t)0x0796, (int16_t)0x068B, (int16_t)0x05E8, (int16_t)0x0526, (int16_t)0x0526, (int16_t)0x079D, (int16_t)0x0AB3, (int16_t)0x0CC8, (int16_t)0x0C36, + (int16_t)0x0B3C, (int16_t)0x0A48, (int16_t)0x093E, (int16_t)0x07F0, (int16_t)0x0713, (int16_t)0x062F, (int16_t)0x0586, (int16_t)0x04D3, (int16_t)0x04D3, (int16_t)0x079D, + (int16_t)0x0AB3, (int16_t)0x0CC8, (int16_t)0x0C36, (int16_t)0x0B3C, (int16_t)0x0A48, (int16_t)0x08E5, (int16_t)0x079A, (int16_t)0x06D0, (int16_t)0x0601, (int16_t)0x055B, + (int16_t)0x04B2, (int16_t)0x04B2, (int16_t)0x079D, (int16_t)0x0AB3, (int16_t)0x0CC8, (int16_t)0x0BE0, (int16_t)0x0AF3, (int16_t)0x0A0E, (int16_t)0x08B8, (int16_t)0x0770, + (int16_t)0x06B0, (int16_t)0x05E9, (int16_t)0x0549, (int16_t)0x0499, (int16_t)0x0499, (int16_t)0x079B, (int16_t)0x0AB1, (int16_t)0x0C12, (int16_t)0x0B37, (int16_t)0x0A62, + (int16_t)0x0991, (int16_t)0x0858, (int16_t)0x0728, (int16_t)0x0676, (int16_t)0x05BB, (int16_t)0x0514, (int16_t)0x0476, (int16_t)0x0476, (int16_t)0x0799, (int16_t)0x0AAF, + (int16_t)0x0AFB, (int16_t)0x0A4B, (int16_t)0x0997, (int16_t)0x08E3, (int16_t)0x07C8, (int16_t)0x06BA, (int16_t)0x061E, (int16_t)0x0575, (int16_t)0x04C4, (int16_t)0x0441, + (int16_t)0x0441, (int16_t)0x0796, (int16_t)0x0AAC, (int16_t)0x0A25, (int16_t)0x0977, (int16_t)0x08CF, (int16_t)0x0826, (int16_t)0x072F, (int16_t)0x0647, (int16_t)0x05BE, + (int16_t)0x0522, (int16_t)0x0474, (int16_t)0x0412, (int16_t)0x0412, (int16_t)0x0794, (int16_t)0x09C2, (int16_t)0x0944, (int16_t)0x089A, (int16_t)0x0818, (int16_t)0x078D, + (int16_t)0x06AF, (int16_t)0x05EE, (int16_t)0x0566, (int16_t)0x04E2, (int16_t)0x0434, (int16_t)0x03DA, (int16_t)0x03DA, (int16_t)0x0792, (int16_t)0x08D7, (int16_t)0x0876, + (int16_t)0x07EC, (int16_t)0x076D, (int16_t)0x06FF, (int16_t)0x064A, (int16_t)0x0586, (int16_t)0x0523, (int16_t)0x04A0, (int16_t)0x03FD, (int16_t)0x03B6, (int16_t)0x03B6, + (int16_t)0x0792, (int16_t)0x083A, (int16_t)0x07DB, (int16_t)0x076F, (int16_t)0x070F, (int16_t)0x069B, (int16_t)0x05EA, (int16_t)0x0523, (int16_t)0x04AB, (int16_t)0x043D, + (int16_t)0x03C6, (int16_t)0x0394, (int16_t)0x0394, (int16_t)0x0792, (int16_t)0x07D1, (int16_t)0x077A, (int16_t)0x0712, (int16_t)0x06B5, (int16_t)0x0654, (int16_t)0x0594, + (int16_t)0x04D9, (int16_t)0x0468, (int16_t)0x03FD, (int16_t)0x035F, (int16_t)0x0304, (int16_t)0x0304, (int16_t)0x0792, (int16_t)0x07AB, (int16_t)0x074E, (int16_t)0x06CF, + (int16_t)0x067A, (int16_t)0x061B, (int16_t)0x0568, (int16_t)0x04B1, (int16_t)0x0444, (int16_t)0x03BB, (int16_t)0x0332, (int16_t)0x02D1, (int16_t)0x02D1, (int16_t)0x0792, + (int16_t)0x0773, (int16_t)0x0710, (int16_t)0x069A, (int16_t)0x0640, (int16_t)0x05CE, (int16_t)0x051F, (int16_t)0x0479, (int16_t)0x0418, (int16_t)0x0390, (int16_t)0x0321, + (int16_t)0x02C0, (int16_t)0x02C0, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, + (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000, (int16_t)0x0000 +}; + +/* 2-D angle-kick combine (RPM x demand) -- consumed by orphan compute_angle_kick_2d @ 0x6AC9 (T06211 analog FUN_5D58 @ 0x5D8D) -- 195 words @ 0x835C. */ +static const int16_t phi_angle_2d_kick_table[195] = { + (int16_t)0x0C16, (int16_t)0x0C16, (int16_t)0x0C16, (int16_t)0x0C16, (int16_t)0x0B8E, (int16_t)0x0C42, (int16_t)0x0957, (int16_t)0x066D, (int16_t)0x074F, (int16_t)0x0831, + (int16_t)0x05F6, (int16_t)0x05F6, (int16_t)0x05F6, (int16_t)0x0C16, (int16_t)0x0C16, (int16_t)0x0C16, (int16_t)0x0C16, (int16_t)0x0B8E, (int16_t)0x0C42, (int16_t)0x0957, + (int16_t)0x066D, (int16_t)0x074F, (int16_t)0x0831, (int16_t)0x05F6, (int16_t)0x05F6, (int16_t)0x05F6, (int16_t)0x0C16, (int16_t)0x0C16, (int16_t)0x0C16, (int16_t)0x0C16, + (int16_t)0x0B8E, (int16_t)0x0C42, (int16_t)0x0957, (int16_t)0x066D, (int16_t)0x074F, (int16_t)0x0831, (int16_t)0x05F6, (int16_t)0x05F6, (int16_t)0x05F6, (int16_t)0x0C16, + (int16_t)0x0C16, (int16_t)0x0C16, (int16_t)0x0C16, (int16_t)0x0B8E, (int16_t)0x0C42, (int16_t)0x0957, (int16_t)0x066D, (int16_t)0x074F, (int16_t)0x0831, (int16_t)0x05F6, + (int16_t)0x05F6, (int16_t)0x05F6, (int16_t)0x0A33, (int16_t)0x0A33, (int16_t)0x0A33, (int16_t)0x0A33, (int16_t)0x09A2, (int16_t)0x0A10, (int16_t)0x07C0, (int16_t)0x0570, + (int16_t)0x0682, (int16_t)0x0795, (int16_t)0x054C, (int16_t)0x054C, (int16_t)0x054C, (int16_t)0x075F, (int16_t)0x075F, (int16_t)0x075F, (int16_t)0x075F, (int16_t)0x06C0, + (int16_t)0x06C6, (int16_t)0x055D, (int16_t)0x03F4, (int16_t)0x054F, (int16_t)0x06AB, (int16_t)0x044D, (int16_t)0x044D, (int16_t)0x044D, (int16_t)0x0481, (int16_t)0x0481, + (int16_t)0x0481, (int16_t)0x0481, (int16_t)0x0542, (int16_t)0x04A0, (int16_t)0x03A9, (int16_t)0x02B3, (int16_t)0x0372, (int16_t)0x0431, (int16_t)0x04AE, (int16_t)0x04AE, + (int16_t)0x04AE, (int16_t)0x01A3, (int16_t)0x01A3, (int16_t)0x01A3, (int16_t)0x01A3, (int16_t)0x03C4, (int16_t)0x027A, (int16_t)0x01F6, (int16_t)0x0172, (int16_t)0x0194, + (int16_t)0x01B7, (int16_t)0x0510, (int16_t)0x0510, (int16_t)0x0510, (int16_t)0x016B, (int16_t)0x016B, (int16_t)0x016B, (int16_t)0x016B, (int16_t)0x02CB, (int16_t)0x013D, + (int16_t)0x0141, (int16_t)0x0146, (int16_t)0x01A2, (int16_t)0x01FF, (int16_t)0x0421, (int16_t)0x0421, (int16_t)0x0421, (int16_t)0x0133, (int16_t)0x0133, (int16_t)0x0133, + (int16_t)0x0133, (int16_t)0x01D3, (int16_t)0x0000, (int16_t)0x008D, (int16_t)0x011A, (int16_t)0x01B0, (int16_t)0x0247, (int16_t)0x0333, (int16_t)0x0333, (int16_t)0x0333, + (int16_t)0x0133, (int16_t)0x0133, (int16_t)0x0133, (int16_t)0x0133, (int16_t)0x01D3, (int16_t)0x0000, (int16_t)0x008D, (int16_t)0x011A, (int16_t)0x01B0, (int16_t)0x0247, + (int16_t)0x0333, (int16_t)0x0333, (int16_t)0x0333, (int16_t)0x0133, (int16_t)0x0133, (int16_t)0x0133, (int16_t)0x0133, (int16_t)0x01D3, (int16_t)0x0000, (int16_t)0x008D, + (int16_t)0x011A, (int16_t)0x01B0, (int16_t)0x0247, (int16_t)0x0333, (int16_t)0x0333, (int16_t)0x0333, (int16_t)0x0133, (int16_t)0x0133, (int16_t)0x0133, (int16_t)0x0133, + (int16_t)0x01D3, (int16_t)0x0000, (int16_t)0x008D, (int16_t)0x011A, (int16_t)0x01B0, (int16_t)0x0247, (int16_t)0x0333, (int16_t)0x0333, (int16_t)0x0333, (int16_t)0x0133, + (int16_t)0x0133, (int16_t)0x0133, (int16_t)0x0133, (int16_t)0x01D3, (int16_t)0x0000, (int16_t)0x008D, (int16_t)0x011A, (int16_t)0x01B0, (int16_t)0x0247, (int16_t)0x0333, + (int16_t)0x0333, (int16_t)0x0333, (int16_t)0x0133, (int16_t)0x0133, (int16_t)0x0133, (int16_t)0x0133, (int16_t)0x01D3, (int16_t)0x0000, (int16_t)0x008D, (int16_t)0x011A, + (int16_t)0x01B0, (int16_t)0x0247, (int16_t)0x0333, (int16_t)0x0333, (int16_t)0x0333 +}; + +/* ====================================================================== + * Master calibration literal. + * ====================================================================== */ + +calibration_t phi_t06215_cal = { + /* RWA4-relative scalars (base 0x9BD8) */ + .tein_nominal = (int16_t)0x0C28, /* CAL+0x1E @ 0x9BF6 -- tein_nominal — nominal TE_IN base added to tein_valve_fault_guard / tein_overtemp_guard (T06211 FUN_7453 0x747b; T06215 FUN_74EA -- see README) */ + .cal_48 = (int16_t)0x04B0, /* CAL+0x48 @ 0x9C20 -- RW7A latch value when FUN_62a2 fires (0x62d8) */ + .phi0 = (int16_t)0x0DAB, /* CAL+0x4C @ 0x9C24 -- phi0: base/initial angle. ROM addend baked into phi1 (= *(0x014e)) by FUN_6aaf (0x6ad2); T06211 FUN_7453 reads phi0 + dphi at 0x74a2 */ + .cal_byte_9c = 0x03u, /* CAL+0x9C @ 0x9C74 -- FUN_62a2 counter increment (0x62c6) */ + .cal_54 = (int16_t)0x0B2B, /* CAL+0x54 @ 0x9C2C -- UPPER clamp on target_inj_angle (T06211 FUN_7453 0x7493) */ + .cal_74 = (int16_t)0x20C5, /* CAL+0x74 @ 0x9C4C -- gate_0220 lower RPM threshold */ + .cal_76 = (int16_t)0x01A3, /* CAL+0x76 @ 0x9C4E -- gate_0220 hysteresis width (upper = cal_74 + cal_76) */ + .cal_78 = (int16_t)0x007F, /* CAL+0x78 @ 0x9C50 -- accel_comp_offset upper clamp (FUN_732d 0x7340) */ + .cal_7a = (int16_t)0xFFCF, /* CAL+0x7A @ 0x9C52 -- accel_comp_offset lower clamp (FUN_732d 0x734e) */ + .cal_7e = (int16_t)0x13AA, /* CAL+0x7E @ 0x9C56 -- temperature reference subtrahend in compute_temp_comp_factor (calc_temp_comp_factor @ 0x6B0D; T06211 analog FUN_5DAB 0x5DD1) */ + .cal_temp_comp_switch_dynamic = (int16_t)0x1702, /* CAL+0x80 @ 0x9C58 -- boot value of temp_comp_dynamic = TK_AT_W switch (compute_temp_comp_factor 0x6AF0; T06211 analog 0x5DB4) */ + .cal_82 = (int16_t)0x026F, /* CAL+0x82 @ 0x9C5A -- IIR input gain b for compute_temp_phi_comp (FUN_6b4e MULU at 0x6B66; Q16 unsigned) */ + .cal_84 = (int16_t)0xFD91, /* CAL+0x84 @ 0x9C5C -- IIR pole coefficient a for compute_temp_phi_comp (FUN_6b4e MULU at 0x6B4E and 0x6B57; Q16 unsigned) */ + .cal_temp_comp_switch_complete = (int16_t)0x0001, /* CAL+0x86 @ 0x9C5E -- boot value of temp_comp_complete = F_TK_TE_W switch (compute_temp_comp_factor 0x6B17; T06211 analog 0x5DDB) */ + .cal_92 = (int16_t)0x1750, /* CAL+0x92 @ 0x9C6A -- temperature offset subtracted in FUN_5ca1 (0x5cf7) */ + .cal_94 = (int16_t)0x0008, /* CAL+0x94 @ 0x9C6C -- FUN_5ca1 multiplier (0x5d05) */ + .cal_96 = (int16_t)0x04B0, /* CAL+0x96 @ 0x9C6E -- FUN_5ca1 RW1C upper clamp (0x5d0b, unsigned) */ + .cal_98 = (int16_t)0x1D7E, /* CAL+0x98 @ 0x9C70 -- tein_overtemp_guard lower RPM (FUN_5ca1 0x5d1a, unsigned) */ + .cal_9a = (int16_t)0x3127, /* CAL+0x9A @ 0x9C72 -- tein_overtemp_guard upper RPM (FUN_5ca1 0x5d21, unsigned) */ + /* RWC6-relative scalars (base 0x7E56) */ + .cal_rwc6_34 = (int16_t)0x0831, /* RWC6+0x34 @ 0x7E8A -- demand-weight multiplier in compute_angle_kick_2d (orphan @ 0x6ABD; T06211 analog FUN_5D58 @ 0x5D81) */ + + /* Accel descriptors (input_var bound at runtime by phi_t06215_bind_inputs). */ + .desc_accel_rpm = { + .runtime_slot = 0, + .input_var = NULL, + .stride_items = 7, + .axis = phi_accel_axis_rpm, + }, + .desc_accel_demand = { + .runtime_slot = 0, + .input_var = NULL, + .stride_items = 6, + .axis = phi_accel_axis_demand, + }, + .desc_accel_temp = { + .runtime_slot = 0, + .input_var = NULL, + .stride_items = 5, + .axis = phi_accel_axis_temp, + }, + + /* Accel table pointers. */ + .accel_combine_table = phi_accel_combine_table, + .accel_refine_table = phi_accel_refine_table, + + /* Absolute-address ROM constants. */ + .dat_604c = (int16_t)0x0444, /* *(0x604C) -- FUN_62a2 RWC2 timing threshold (T06211 = 0x0444; verify in T06215) */ + .cal_byte_402 = (int16_t)0xFFF2, /* *(0x0402) -- FUN_6ba3 sign-extends byte 0x0402 (compute_temp_comp_factor 0x6B12; T06211 analog 0x5DD6) */ + + /* Angle accumulator descriptors (FUN_722e, RWC6-relative; input_var bound at runtime). */ + .desc_rpm = { + .runtime_slot = 0, + .input_var = NULL, + .stride_items = 13, + .axis = phi_angle_axis_rpm, + }, + .desc_demand = { + .runtime_slot = 0, + .input_var = NULL, + .stride_items = 15, + .axis = phi_angle_axis_demand, + }, + .desc_dec_cmd = { + .runtime_slot = 0, + .input_var = NULL, + .stride_items = 3, + .axis = phi_angle_axis_dec_cmd, + }, + + /* Angle accumulator 3-D table pointer. */ + .data_table_3d = phi_angle_3d_table, + .data_table_2d_kick = phi_angle_2d_kick_table, +}; + +/* ====================================================================== + * Runtime input binder. + * ====================================================================== */ + +void phi_t06215_bind_inputs(runtime_state_t *rt, calibration_t *cal) +{ + cal->desc_accel_rpm.input_var = (int16_t *)&rt->rpm; + cal->desc_accel_demand.input_var = &rt->inj_qty_demand; + cal->desc_accel_temp.input_var = &rt->temperature; + + cal->desc_rpm.input_var = (int16_t *)&rt->rpm; + cal->desc_demand.input_var = &rt->inj_qty_demand; + cal->desc_dec_cmd.input_var = &rt->angle_dec_cmd; +} diff --git a/Core/Src/fuel_map.c b/Core/Src/fuel_map.c index 6894016..f22e7a9 100644 --- a/Core/Src/fuel_map.c +++ b/Core/Src/fuel_map.c @@ -1,885 +1,112 @@ /* - * fuel_map.c + * fuel_map.c — T06215 variant * - * Created on: Oct 18, 2024 - * Author: herli + * Thin wrapper that exposes the legacy host-app API + * (init_FuelMap / FM_GET_PHIAD) on top of the reverse-engineered + * T06215 phi pipeline. The original float trilinear interpolation + * (5 fuel maps × RPM × ME plus a cosine empirical correction) has + * been removed — the phi pipeline is now the sole angle-calculation + * source. + * + * Mapping: + * FM_GET_PHIAD(RPM, ME, Temp) -> phi_outputs_t.angle_accumulator + * (RW52, "0052 accumulator") + * scaled to float degrees (/ 85.333). + * + * Getter scale factors mirror example_interface_getters.txt: + * RPM raw = real_rpm × 8.388 (CF_TMS) + * ME raw = real_me × 32 (CF_ME) + * Temp raw = real_°C × 16 + 4368 (CF_T_M, CF_T_N) */ -#include #include "fuel_map.h" +#include "phi.h" +#include +#include -struct fuelMapIndexes indexes; +#define CF_TMS 8.388f +#define CF_ME 32.0f +#define CF_T_M 16.0f +#define CF_T_N 4368.0f +#define CF_KW 85.3333f +#define ANGLE_DEG_PER_RAW (1.0f / 85.333f) -float* phi_pointer; +/* Per-call input cache. FM_GET_PHIAD's float args land here so the + * nullary phi getters can read them. */ +static float s_rpm = 0.0f; +static float s_me = 0.0f; +static float s_temp = 0.0f; +/* Encapsulated phi runtime — opaque to the caller. */ +static phi_state_t s_phi_state; +static phi_cal_t s_phi_cal; +static phi_input_getters_t s_phi_getters; +static phi_outputs_t s_phi_out; +static float *s_phiad_out = NULL; -void init_FuelMap(float* PHIAD) { - //fuelmap = fuelmap_25; - //fuelmap2 = fuelmap_74; - indexes = fuelMapI; - phi_pointer = PHIAD; +/* ──── Getters (nullary; T06215 vtable shape) ────────────────────────── */ + +static uint16_t get_rpm(void) { return (uint16_t)(s_rpm * CF_TMS); } +static int16_t get_inj_qty_demand(void) { return (int16_t) (s_me * CF_ME); } +static int16_t get_temperature(void) { return (int16_t) (s_temp * CF_T_M + CF_T_N); } + +/* angle-decrease CAN command. Default 0 matches sim_t06215_sweep.c. + * To wire this to a host-app global, replace the body with e.g.: + * extern float B_PHIAD; + * return (int16_t)(B_PHIAD * CF_KW); + */ +static int16_t get_angle_dec_cmd(void) { return 0; } + +/* Steady-state default: baseline tracks current RPM, so Δ-rpm = 0 and + * the accel-comp branch contributes nothing. Override if modelling + * transients. */ +static int16_t get_rpm_baseline(void) { return (int16_t)(s_rpm * CF_TMS); } + +static uint16_t get_rwc2(void) { return 0; } /* slow timing scratch */ +static uint8_t get_reset_gate_0226(void) { return 0xFF; } /* accel-comp enabled */ +static int16_t get_dphi(void) { return 0; } +static uint8_t get_scratch_0103(void) { return 0xFF; } /* matches sim_t06215_sweep.c */ +static uint8_t get_scratch_0108(void) { return 0xFF; } + +/* ──── Public API ─────────────────────────────────────────────────────── */ + +void init_FuelMap(float *PHIAD) { + s_phiad_out = PHIAD; + + s_phi_getters.get_rpm = get_rpm; + s_phi_getters.get_inj_qty_demand = get_inj_qty_demand; + s_phi_getters.get_temperature = get_temperature; + s_phi_getters.get_angle_dec_cmd = get_angle_dec_cmd; + s_phi_getters.get_rpm_baseline = get_rpm_baseline; + s_phi_getters.get_rwc2 = get_rwc2; + s_phi_getters.get_reset_gate_0226 = get_reset_gate_0226; + s_phi_getters.get_dphi = get_dphi; + s_phi_getters.get_scratch_0103 = get_scratch_0103; + s_phi_getters.get_scratch_0108 = get_scratch_0108; + + s_phi_cal = phi_t06215_cal; + phi_init(&s_phi_state, &s_phi_cal, &s_phi_getters); + + /* Pre-latch to skip the cranking-phase ramp (mirrors + * sim_t06215_sweep.c:237-238). With this in place, the very first + * phi_service call enters FUN_62a2 in Phase 2 and tein_valve_fault_guard + * holds at cal_48 from boot. */ + s_phi_state.rt.tein_valve_fault_guard = s_phi_cal.cal_48; + s_phi_state.rt.scratch_010f = s_phi_state.rt.scratch_0108; } -/*float LinearInterp_noT(float var, float Indexarray[], int N, int OBIndex, int isRPM){ - float Z = 0; - for(int i = 0; i < N; i++){ - if(var >= Indexarray[i] && var < Indexarray[i+1]){ - if(isRPM){ - Z = fuelmap.ME_RPM_Beta_array[OBIndex][i] + ((fuelmap.ME_RPM_Beta_array[OBIndex][i+1] - fuelmap.ME_RPM_Beta_array[OBIndex][i]) / - (indexes.RPM_Index_array[i+1] - indexes.RPM_Index_array[i])) * (var - indexes.RPM_Index_array[i]); - }else{ - Z = fuelmap.ME_RPM_Beta_array[i][OBIndex] + ((fuelmap.ME_RPM_Beta_array[i+1][OBIndex] - fuelmap.ME_RPM_Beta_array[i][OBIndex]) / - (indexes.ME_Index_array[i+1] - indexes.ME_Index_array[i])) * (var - indexes.ME_Index_array[i]); - } - break; - } - } - return Z; -}*/ -/*float LinearInterp(float var, float Indexarray[], int N, int OBIndex, int isRPM){ //para la transpuesta - float Z = 0; - for(int i = 0; i < N; i++){ - if(var >= Indexarray[i] && var < Indexarray[i+1]){ - if(isRPM){ - Z = fuelmap.ME_RPM_Beta_array[i][OBIndex] + ((fuelmap.ME_RPM_Beta_array[i+1][OBIndex] - fuelmap.ME_RPM_Beta_array[i][OBIndex]) / - (indexes.RPM_Index_array[i+1] - indexes.RPM_Index_array[i])) * (var - indexes.RPM_Index_array[i]); - }else{ - Z = fuelmap.ME_RPM_Beta_array[OBIndex][i] + ((fuelmap.ME_RPM_Beta_array[OBIndex][i+1] - fuelmap.ME_RPM_Beta_array[OBIndex][i]) / - (indexes.ME_Index_array[i+1] - indexes.ME_Index_array[i])) * (var - indexes.ME_Index_array[i]); - } - break; - } - } - return Z; -}*/ - -static inline void SelectTempBracket(const struct fuelMapIndexes* idx, float Temp, int* t_lo, int* t_hi, float* T1, float* T2, float* w) -{ - // clamp Temp into axis - if (Temp <= idx->T_Index_array[0]) { - *t_lo = *t_hi = 0; - } else if (Temp >= idx->T_Index_array[FM_N_T - 1]) { - *t_lo = *t_hi = FM_N_T - 1; - } else { - int i = 0; - // find i such that T[i] <= Temp < T[i+1] - while (i + 1 < FM_N_T && Temp >= idx->T_Index_array[i + 1]) i++; - *t_lo = i; - *t_hi = i + 1; - } - - *T1 = idx->T_Index_array[*t_lo]; - *T2 = idx->T_Index_array[*t_hi]; - float denom = (*T2 - *T1); - *w = (denom != 0.0f) ? (Temp - *T1) / denom : 0.0f; // blend weight in [0,1] +void Timer1_FM_ISR(){ + phi_tick_1khz(&s_phi_state, &s_phi_cal); } -float weirdasscorrection; -float GetAlpha(float RPM, float ME, float Tein, float Temp){ - int RPMBounds = -1; // (-1)-Central Point, 0-Outside Minimum, 1-Outside Maximum - int MEBounds = -1; +extern float forceTemp; +float FM_GET_PHIAD(float RPM, float ME, float Temp) { + s_rpm = RPM; + s_me = ME; + s_temp = Temp; - int I_RPM = 0; - int I_ME = 0; + phi_service(&s_phi_state, &s_phi_cal, &s_phi_out); - float Z = 0; - if(ME < 0.031){//0.031 - *phi_pointer = 0.0; - return 0.0; - } - - if(RPM >= indexes.RPM_Index_array[FM_N_RPM-1]){ //MAX RPM - RPMBounds = 1; - I_RPM = FM_N_RPM-2; - //RPM = indexes.RPM_Index_array[FM_N_RPM-1]; - } - if(ME >= indexes.ME_Index_array[FM_N_ME-1]){ //MAX ME - MEBounds = 1; - I_ME = FM_N_ME-2; - //ME = indexes.ME_Index_array[FM_N_ME-1]; - } - if(RPM <= indexes.RPM_Index_array[0]){ - RPMBounds = 0; - I_RPM = 0; - //RPM = indexes.RPM_Index_array[0]; - } - if(ME <= indexes.ME_Index_array[0]){ - MEBounds = 0; - I_ME = 0; - //ME = indexes.ME_Index_array[0]; - } - if(RPMBounds == -1){ - for(int i = 0; i < FM_N_RPM-1; i++){ - if(RPM >= indexes.RPM_Index_array[i] && RPM < indexes.RPM_Index_array[i+1]){ - I_RPM = i; - break; - } - } - } - if(MEBounds == -1){ - for(int i = 0; i < FM_N_ME-1; i++){ - if(ME >= indexes.ME_Index_array[i] && ME < indexes.ME_Index_array[i+1]){ - I_ME = i; - break; - } - } - } - // --- now: pick them from the array based on Temp - int t_lo, t_hi; - float FM_T_1, FM_T_2, wT; - SelectTempBracket(&fuelMapI, Temp, &t_lo, &t_hi, &FM_T_1, &FM_T_2, &wT); - - const struct AlphaStruct* fuelmap_low = g_FuelMaps[t_lo]; - const struct AlphaStruct* fuelmap_high = g_FuelMaps[t_hi]; - - // ---- your existing bilinear on the LOW temp map ---- - float X1 = indexes.RPM_Index_array[I_RPM]; - float X2 = indexes.RPM_Index_array[I_RPM+1]; - float Y1 = indexes.ME_Index_array[I_ME]; - float Y2 = indexes.ME_Index_array[I_ME + 1]; - - float Z1 = fuelmap_low->ME_RPM_Beta_array[I_RPM ][I_ME ]; - float Z2 = fuelmap_low->ME_RPM_Beta_array[I_RPM + 1][I_ME ]; - float Z3 = fuelmap_low->ME_RPM_Beta_array[I_RPM ][I_ME + 1]; - float Z4 = fuelmap_low->ME_RPM_Beta_array[I_RPM + 1][I_ME + 1]; - - float Z_low = (1.0f / ((X2 - X1) * (Y2 - Y1))) * - ( Z1 * (X2 - RPM) * (Y2 - ME) - + Z2 * (RPM - X1) * (Y2 - ME) - + Z3 * (X2 - RPM) * (ME - Y1) - + Z4 * (RPM - X1) * (ME - Y1) ); - - // ---- the same bilinear on the HIGH temp map ---- - Z1 = fuelmap_high->ME_RPM_Beta_array[I_RPM ][I_ME ]; - Z2 = fuelmap_high->ME_RPM_Beta_array[I_RPM + 1][I_ME ]; - Z3 = fuelmap_high->ME_RPM_Beta_array[I_RPM ][I_ME + 1]; - Z4 = fuelmap_high->ME_RPM_Beta_array[I_RPM + 1][I_ME + 1]; - - float Z_high = (1.0f / ((X2 - X1) * (Y2 - Y1))) * - ( Z1 * (X2 - RPM) * (Y2 - ME) - + Z2 * (RPM - X1) * (Y2 - ME) - + Z3 * (X2 - RPM) * (ME - Y1) - + Z4 * (RPM - X1) * (ME - Y1) ); - - // ---- final: identical blending idea you had before, but generic ---- - Z = (t_lo == t_hi) ? Z_low : (Z_low + wT * (Z_high - Z_low)); - - /*if(MEBounds != -1 && RPMBounds != -1){ //if Out of Two Bounds --> Get the Corner value - I_RPM = RPMBounds * (FM_N_RPM - 1); // 0 Minimum, 1*Max Value - I_ME = MEBounds * (FM_N_ME -1); - //Z = fuelmap.ME_RPM_Beta_array[I_RPM][I_ME]; //la transpuesta - }else if(MEBounds != -1 || RPMBounds != -1){ - if(RPMBounds == -1){ //If Out of Bounds in ME - Z = LinearInterp(RPM, indexes.RPM_Index_array, FM_N_RPM, MEBounds * (FM_N_ME -1), 1); - }else{ - Z = LinearInterp(ME, indexes.ME_Index_array, FM_N_ME, RPMBounds * (FM_N_RPM - 1), 0); - } - }else{ //CENTER VALUES - for(int i = 0; i < FM_N_RPM; i++){ - if(RPM >= indexes.RPM_Index_array[i] && RPM < indexes.RPM_Index_array[i+1]){ - I_RPM = i; - break; - } - } - for(int i = 0; i < FM_N_ME; i++){ - if(ME >= indexes.ME_Index_array[i] && ME < indexes.ME_Index_array[i+1]){ - I_ME = i; - break; - } - } - float X1 = indexes.RPM_Index_array[I_RPM]; - float X2 = indexes.RPM_Index_array[I_RPM+1]; - float Y1 = indexes.ME_Index_array[I_ME]; - float Y2 = indexes.ME_Index_array[I_ME+1]; - - float Z1 = fuelmap.ME_RPM_Beta_array[I_RPM][I_ME]; - float Z2 = fuelmap.ME_RPM_Beta_array[I_RPM+1][I_ME]; - float Z3 = fuelmap.ME_RPM_Beta_array[I_RPM][I_ME+1]; - float Z4 = fuelmap.ME_RPM_Beta_array[I_RPM+1][I_ME+1]; - Z = (1.0 / ((X2 - X1) * (Y2 - Y1))) * ( Z1 * (X2 - RPM) * (Y2 - ME) + Z2 * (RPM - X1) * (Y2 - ME) + Z3 * (X2 - RPM) * (ME - Y1) + Z4 * (RPM - X1) * (ME - Y1) ); - - Z1 = fuelmap2.ME_RPM_Beta_array[I_RPM][I_ME]; - Z2 = fuelmap2.ME_RPM_Beta_array[I_RPM+1][I_ME]; - Z3 = fuelmap2.ME_RPM_Beta_array[I_RPM][I_ME+1]; - Z4 = fuelmap2.ME_RPM_Beta_array[I_RPM+1][I_ME+1]; - Z = Z + ((Temp - FM_T_1) / (FM_T_2 - FM_T_1)) * ((1.0 / ((X2 - X1) * (Y2 - Y1))) * ( Z1 * (X2 - RPM) * (Y2 - ME) + Z2 * (RPM - X1) * (Y2 - ME) + Z3 * (X2 - RPM) * (ME - Y1) + Z4 * (RPM - X1) * (ME - Y1)) - Z); - }*/ - //Z -= RPM / 256 + 1.171875; //Calibrated PHIAD - //Z += correctedTein*RPM*USTODEG; //PHIAD + Tein correction - weirdasscorrection = RPM > 300 && RPM < 1838 ? 0.13*cos(0.0041*RPM+3.1415*1.1) : 0; - Z -= weirdasscorrection; - - Z = Z < 0 ? 0 : Z; - - *phi_pointer = Z; - return Z + (Tein - TEIN_NOMINAL)*RPM*USTODEG; //en el 504012 el ref era en 1500, en la vp30 004005 es 1000; + float Z = (float)s_phi_out.angle_accumulator * ANGLE_DEG_PER_RAW; + if (s_phiad_out) *s_phiad_out = Z; + return Z; } -float FM_GET_PHIAD(float RPM, float ME, float Temp){ - int RPMBounds = -1; // (-1)-Central Point, 0-Outside Minimum, 1-Outside Maximum - int MEBounds = -1; - - int I_RPM = 0; - int I_ME = 0; - - float Z = 0; - if(ME < 0.031){//0.031 - *phi_pointer = 0.0; - return 0.0; - } - - if(RPM >= indexes.RPM_Index_array[FM_N_RPM-1]){ //MAX RPM - RPMBounds = 1; - I_RPM = FM_N_RPM-2; - //RPM = indexes.RPM_Index_array[FM_N_RPM-1]; - } - if(ME >= indexes.ME_Index_array[FM_N_ME-1]){ //MAX ME - MEBounds = 1; - I_ME = FM_N_ME-2; - //ME = indexes.ME_Index_array[FM_N_ME-1]; - } - if(RPM <= indexes.RPM_Index_array[0]){ - RPMBounds = 0; - I_RPM = 0; - //RPM = indexes.RPM_Index_array[0]; - } - if(ME <= indexes.ME_Index_array[0]){ - MEBounds = 0; - I_ME = 0; - //ME = indexes.ME_Index_array[0]; - } - if(RPMBounds == -1){ - for(int i = 0; i < FM_N_RPM-1; i++){ - if(RPM >= indexes.RPM_Index_array[i] && RPM < indexes.RPM_Index_array[i+1]){ - I_RPM = i; - break; - } - } - } - if(MEBounds == -1){ - for(int i = 0; i < FM_N_ME-1; i++){ - if(ME >= indexes.ME_Index_array[i] && ME < indexes.ME_Index_array[i+1]){ - I_ME = i; - break; - } - } - } - // --- now: pick them from the array based on Temp - int t_lo, t_hi; - float FM_T_1, FM_T_2, wT; - SelectTempBracket(&fuelMapI, Temp, &t_lo, &t_hi, &FM_T_1, &FM_T_2, &wT); - - const struct AlphaStruct* fuelmap_low = g_FuelMaps[t_lo]; - const struct AlphaStruct* fuelmap_high = g_FuelMaps[t_hi]; - - // ---- your existing bilinear on the LOW temp map ---- - float X1 = indexes.RPM_Index_array[I_RPM]; - float X2 = indexes.RPM_Index_array[I_RPM+1]; - float Y1 = indexes.ME_Index_array[I_ME]; - float Y2 = indexes.ME_Index_array[I_ME + 1]; - - float Z1 = fuelmap_low->ME_RPM_Beta_array[I_RPM ][I_ME ]; - float Z2 = fuelmap_low->ME_RPM_Beta_array[I_RPM + 1][I_ME ]; - float Z3 = fuelmap_low->ME_RPM_Beta_array[I_RPM ][I_ME + 1]; - float Z4 = fuelmap_low->ME_RPM_Beta_array[I_RPM + 1][I_ME + 1]; - - float Z_low = (1.0f / ((X2 - X1) * (Y2 - Y1))) * - ( Z1 * (X2 - RPM) * (Y2 - ME) - + Z2 * (RPM - X1) * (Y2 - ME) - + Z3 * (X2 - RPM) * (ME - Y1) - + Z4 * (RPM - X1) * (ME - Y1) ); - - // ---- the same bilinear on the HIGH temp map ---- - Z1 = fuelmap_high->ME_RPM_Beta_array[I_RPM ][I_ME ]; - Z2 = fuelmap_high->ME_RPM_Beta_array[I_RPM + 1][I_ME ]; - Z3 = fuelmap_high->ME_RPM_Beta_array[I_RPM ][I_ME + 1]; - Z4 = fuelmap_high->ME_RPM_Beta_array[I_RPM + 1][I_ME + 1]; - - float Z_high = (1.0f / ((X2 - X1) * (Y2 - Y1))) * - ( Z1 * (X2 - RPM) * (Y2 - ME) - + Z2 * (RPM - X1) * (Y2 - ME) - + Z3 * (X2 - RPM) * (ME - Y1) - + Z4 * (RPM - X1) * (ME - Y1) ); - - // ---- final: identical blending idea you had before, but generic ---- - Z = (t_lo == t_hi) ? Z_low : (Z_low + wT * (Z_high - Z_low)); - - weirdasscorrection = RPM > 300 && RPM < 1838 ? 0.13*cos(0.0041*RPM+3.1415*1.1) : 0; - Z -= weirdasscorrection; - - Z = Z < -12 ? 0 : Z; - *phi_pointer = Z; - return Z; -} -float GetBeta(float inRPM, float Tein){ - //float extraTein = Tein > 800 ? Tein - 800 : Tein ; - return PHI1 - Tein*inRPM*USTODEG; -} - - -#define BOO_MIN_RPM_1 600 -#define BOO_MAX_RPM_1 2000 -#define BOO_MAX_BOOST_1 0.6 -#define BOO_MIN_ME_1 16 -#define BOO_MAX_ME_1 30 - -float bp_1 = -4*BOO_MAX_BOOST_1/((BOO_MAX_RPM_1-BOO_MIN_RPM_1)*(BOO_MAX_RPM_1-BOO_MIN_RPM_1)); - - -#define BOO_MIN_RPM_2 500 -#define BOO_MAX_RPM_2 2000 -#define BOO_MAX_BOOST_2 0.8 -#define BOO_MIN_ME_2 10 -#define BOO_MAX_ME_2 18 - -float bp_2 = -4*BOO_MAX_BOOST_2/((BOO_MAX_RPM_2-BOO_MIN_RPM_2)*(BOO_MAX_RPM_2-BOO_MIN_RPM_2)); - -float BoostMultiplier(uint8_t mode, float RPM, float ME){ - // https://www.desmos.com/calculator/euezpayzxb - float m = 1.0; - switch (mode) { - case 0: - m = 1.0; - break; - case 1: //boost - float a1 = RPM-BOO_MIN_RPM_1; - float b1 = RPM-BOO_MAX_RPM_1; - if(a1 > 0 && b1 < 0 && ME > BOO_MIN_ME_1){ - float d = (ME-BOO_MIN_ME_1)/(BOO_MAX_ME_1-BOO_MIN_ME_1); - d = d>1 ? 1 : d; - m = 1 + d*bp_1*a1*b1; - }else{ - m = 1; - } - break; - case 2: //boost 2 - float a2 = RPM-BOO_MIN_RPM_2; - float b2 = RPM-BOO_MAX_RPM_2; - if(a2 > 0 && b2 < 0 && ME > BOO_MIN_ME_2){ - float d = (ME-BOO_MIN_ME_2)/(BOO_MAX_ME_2-BOO_MIN_ME_2); - d = d>1 ? 1 : d; - m = 1 + d*bp_2*a2*b2; - }else{ - m = 1; - } - break; - case 3: //itv - if(RPM > 600) - m = 1 - 0.25*(RPM-600)/(2600-600); - else{ - m = 1; - } - break; - default: - m = 1.0; - break; - } - return m; -} - - - -#if defined(_004006) -struct AlphaStruct fuelmap_m12 = { //probar este mapa - { - { 6.094, 8.250, 8.344, 8.355, 8.414, 8.859, 10.383, 10.781, 11.004, 11.227, 12.598, 13.699}, //RPM = 99 - { 5.555, 7.746, 7.828, 7.934, 8.250, 8.930, 10.887, 11.168, 11.508, 11.859, 13.207, 14.273}, //RPM = 199 - { 4.441, 6.703, 6.879, 7.125, 7.828, 9.000, 11.063, 11.648, 12.293, 12.938, 14.531, 15.820}, //RPM = 424 - { 3.984, 6.281, 6.504, 6.773, 7.535, 8.883, 11.297, 11.930, 12.539, 13.148, 14.859, 16.230}, //RPM = 499 - { 2.824, 5.449, 5.883, 6.129, 7.008, 8.051, 11.801, 12.750, 13.688, 14.602, 17.402, 19.652}, //RPM = 896 - { 2.543, 5.320, 5.695, 5.977, 6.750, 7.945, 11.930, 12.984, 13.980, 14.988, 18.000, 20.426}, //RPM = 999 - { 0.609, 3.609, 4.395, 4.770, 5.871, 6.832, 11.859, 13.148, 14.473, 15.773, 19.711, 22.852}, //RPM = 1399 - { 0.340, 3.469, 4.301, 4.652, 5.660, 6.738, 11.977, 13.301, 14.707, 16.125, 20.379, 23.777}, //RPM = 1499 - { -1.148, 2.414, 3.410, 3.703, 4.488, 5.836, 11.918, 13.324, 15.492, 17.566, 23.496, 24.598}, //RPM = 2002 - { -1.898, 1.840, 2.730, 3.047, 3.961, 5.227, 11.543, 12.938, 15.504, 18.047, 24.598, 24.598}, //RPM = 2199 - } -}; -struct AlphaStruct fuelmap_10 = { //probar este mapa - { - { 6.211, 8.391, 8.484, 8.496, 8.555, 9.012, 10.535, 10.957, 11.180, 11.426, 12.797, 13.898}, //RPM = 99 - { 5.648, 7.875, 7.957, 8.063, 8.379, 9.105, 11.168, 11.496, 11.883, 12.258, 13.605, 14.684}, //RPM = 199 - { 4.559, 6.867, 7.031, 7.254, 7.992, 9.164, 11.332, 11.906, 12.563, 13.207, 14.789, 16.090}, //RPM = 425 - { 4.102, 6.410, 6.621, 6.902, 7.711, 9.047, 11.496, 12.164, 12.820, 13.465, 15.164, 16.512}, //RPM = 499 - { 2.871, 5.566, 5.930, 6.234, 7.090, 8.191, 12.094, 13.113, 14.051, 14.988, 17.777, 19.992}, //RPM = 894 - { 2.590, 5.391, 5.777, 6.059, 6.867, 8.004, 12.246, 13.348, 14.379, 15.398, 18.422, 20.848}, //RPM = 999 - { 0.785, 3.832, 4.523, 4.887, 5.930, 6.996, 12.258, 13.594, 14.918, 16.230, 20.168, 23.320}, //RPM = 1399 - { 0.480, 3.645, 4.406, 4.758, 5.730, 6.879, 12.363, 13.758, 15.176, 16.605, 20.848, 24.246}, //RPM = 1499 - { -1.148, 2.484, 3.434, 3.750, 4.605, 5.965, 12.492, 14.063, 16.113, 18.188, 24.059, 24.598}, //RPM = 2000 - { -1.910, 1.828, 2.730, 3.082, 4.078, 5.402, 12.270, 13.898, 16.313, 18.738, 24.598, 24.598}, //RPM = 2199 - } -}; -struct AlphaStruct fuelmap_25 = { //probar este mapa - { - { 6.316, 8.531, 8.625, 8.637, 8.695, 9.152, 10.688, 11.109, 11.367, 11.625, 12.996, 14.098}, //RPM = 99 - { 5.742, 7.992, 8.086, 8.191, 8.508, 9.270, 11.449, 11.824, 12.246, 12.668, 14.004, 15.082}, //RPM = 199 - { 4.652, 7.008, 7.184, 7.430, 8.145, 9.305, 11.590, 12.164, 12.820, 13.430, 15.059, 16.348}, //RPM = 424 - { 4.195, 6.539, 6.773, 7.055, 7.852, 9.188, 11.730, 12.410, 13.090, 13.723, 15.434, 16.781}, //RPM = 498 - { 2.906, 5.602, 6.012, 6.316, 7.160, 8.285, 12.398, 13.395, 14.402, 15.328, 18.188, 20.379}, //RPM = 894 - { 2.648, 5.484, 5.848, 6.141, 6.926, 8.063, 12.574, 13.711, 14.766, 15.820, 18.844, 21.270}, //RPM = 999 - { 0.949, 4.066, 4.664, 5.004, 5.988, 7.148, 12.656, 14.039, 15.352, 16.699, 20.637, 23.777}, //RPM = 1399 - { 0.609, 3.820, 4.523, 4.852, 5.801, 7.008, 12.762, 14.215, 15.633, 17.074, 21.316, 24.598}, //RPM = 1499 - { -1.160, 2.520, 3.445, 3.797, 4.746, 6.082, 12.984, 14.777, 16.793, 18.785, 24.598, 24.598}, //RPM = 1999 - { -1.898, 1.840, 2.719, 3.117, 4.195, 5.613, 12.973, 14.848, 17.133, 19.430, 24.598, 24.598}, //RPM = 2199 - } -}; -struct AlphaStruct fuelmap_60 = { //probar este mapa - { - { 6.445, 8.719, 8.813, 8.824, 8.883, 9.328, 10.875, 11.309, 11.590, 11.871, 13.242, 14.344}, //RPM = 99 - { 5.859, 8.156, 8.250, 8.344, 8.660, 9.457, 11.789, 12.223, 12.680, 13.148, 14.484, 15.563}, //RPM = 199 - { 4.793, 7.184, 7.371, 7.582, 8.309, 9.480, 11.848, 12.504, 13.125, 13.746, 15.375, 16.641}, //RPM = 425 - { 4.313, 6.691, 6.902, 7.219, 8.074, 9.352, 11.977, 12.715, 13.395, 14.121, 15.797, 17.180}, //RPM = 499 - { 3.000, 5.719, 6.082, 6.387, 7.242, 8.344, 12.727, 13.758, 14.824, 15.785, 18.621, 20.824}, //RPM = 895 - { 2.719, 5.555, 5.941, 6.234, 7.055, 8.133, 12.926, 14.109, 15.199, 16.277, 19.301, 21.727}, //RPM = 999 - { 1.102, 4.313, 4.805, 5.121, 6.047, 7.313, 13.078, 14.508, 15.855, 17.191, 21.129, 24.270}, //RPM = 1399 - { 0.750, 4.020, 4.664, 4.969, 5.883, 7.160, 13.172, 14.695, 16.125, 17.578, 21.820, 24.598}, //RPM = 1499 - { -1.125, 2.508, 3.480, 3.820, 4.840, 6.234, 13.605, 15.527, 17.461, 19.430, 24.598, 24.598}, //RPM = 1999 - { -1.922, 1.852, 2.742, 3.141, 4.324, 5.859, 13.734, 15.844, 17.988, 20.145, 24.598, 24.598}, //RPM = 2199 - } -}; -struct AlphaStruct fuelmap_80 = { //probar este mapa - { - { 6.633, 8.965, 9.059, 9.059, 9.117, 9.574, 11.145, 11.590, 11.895, 12.199, 13.582, 14.672}, //RPM = 99 - { 6.023, 8.367, 8.461, 8.566, 8.883, 9.738, 12.258, 12.773, 13.289, 13.816, 15.164, 16.230}, //RPM = 199 - { 4.957, 7.430, 7.605, 7.840, 8.555, 9.715, 12.281, 12.926, 13.570, 14.180, 15.785, 17.074}, //RPM = 425 - { 4.477, 6.902, 7.125, 7.441, 8.355, 9.598, 12.375, 13.137, 13.840, 14.555, 16.289, 17.672}, //RPM = 499 - { 3.047, 5.813, 6.164, 6.504, 7.359, 8.496, 13.207, 14.320, 15.375, 16.406, 19.254, 21.469}, //RPM = 895 - { 2.801, 5.672, 6.059, 6.363, 7.207, 8.215, 13.441, 14.684, 15.820, 16.945, 19.969, 22.395}, //RPM = 999 - { 1.406, 4.676, 5.016, 5.309, 6.141, 7.559, 13.699, 15.211, 16.570, 17.930, 21.855, 24.598}, //RPM = 1399 - { 0.961, 4.301, 4.816, 5.121, 5.953, 7.371, 13.793, 15.422, 16.863, 18.328, 22.570, 24.598}, //RPM = 1499 - { -1.102, 2.496, 3.516, 3.914, 5.027, 6.480, 14.461, 16.652, 18.527, 20.320, 24.598, 24.598}, //RPM = 1998 - { -1.922, 1.852, 2.742, 3.188, 4.500, 6.105, 14.859, 17.344, 19.242, 21.199, 24.598, 24.598}, //RPM = 2199 - } -}; - -struct fuelMapIndexes fuelMapI = { //probar este mapa - { 100, 200, 425, 500, 895, 1000, 1400, 1500, 2000, 2200}, //N_RPM = 17 - { 0.375, 0.500, 1.625, 2.500, 5.000, 10.000, 30.000, 35.000, 40.000, 45.000, 60.000, 72.000}, - { -10, 10, 30.5, 50, 80} -}; -#elif defined(_004004) -struct AlphaStruct fuelmap_m12 = { //probar este mapa - { - { 5.027, 6.902, 7.020, 7.113, 7.383, 8.074, 10.641, 11.508, 11.684, 11.871, 12.879, 13.688}, //RPM = 99 - { 5.309, 7.488, 7.629, 7.734, 8.051, 8.766, 11.309, 12.117, 12.258, 12.410, 13.371, 14.133}, //RPM = 199 - { 4.242, 6.504, 6.750, 7.031, 7.770, 8.895, 11.004, 11.484, 11.977, 12.480, 13.934, 15.105}, //RPM = 424 - { 3.855, 5.051, 5.473, 5.742, 6.539, 7.676, 11.309, 12.363, 13.254, 14.145, 17.004, 19.289}, //RPM = 999 - { 0.809, 3.891, 4.430, 4.863, 6.105, 7.266, 12.082, 13.512, 14.426, 15.340, 19.582, 22.969}, //RPM = 1399 - { 0.504, 3.691, 4.277, 4.699, 5.848, 7.066, 12.047, 13.500, 14.602, 15.703, 20.168, 23.730}, //RPM = 1499 - { -1.348, 6.129, 6.492, 6.797, 7.617, 8.789, 11.109, 11.695, 12.246, 12.785, 14.531, 15.938}, //RPM = 499 - { 2.578, 5.250, 5.625, 5.930, 6.691, 7.898, 11.262, 12.223, 12.996, 13.863, 16.582, 18.762}, //RPM = 896 - { 2.285, 2.215, 3.164, 3.516, 4.535, 5.801, 12.094, 14.227, 15.785, 17.355, 23.309, 24.598}, //RPM = 1999 - { -2.273, 1.383, 2.438, 2.848, 4.008, 5.145, 12.363, 15.117, 16.523, 17.941, 24.598, 24.598}, //RPM = 2199 - } -}; -struct AlphaStruct fuelmap_m5 = { //probar este mapa - { - { 5.109, 7.020, 7.137, 7.230, 7.500, 8.156, 10.711, 11.520, 11.742, 11.965, 12.961, 13.781}, //RPM = 99 - { 5.367, 7.559, 7.688, 7.793, 8.121, 8.859, 11.379, 12.141, 12.316, 12.504, 13.465, 14.227}, //RPM = 199 - { 4.289, 6.574, 6.797, 7.090, 7.875, 8.977, 11.133, 11.625, 12.117, 12.621, 14.086, 15.246}, //RPM = 424 - { 3.902, 6.152, 6.539, 6.832, 7.699, 8.906, 11.238, 11.848, 12.398, 12.938, 14.695, 16.090}, //RPM = 499 - { 2.590, 5.250, 5.672, 5.977, 6.785, 7.969, 11.449, 12.363, 13.219, 14.063, 16.770, 18.949}, //RPM = 895 - { 2.297, 5.063, 5.484, 5.777, 6.598, 7.711, 11.484, 12.539, 13.453, 14.355, 17.215, 19.500}, //RPM = 999 - { 0.855, 3.949, 4.488, 4.922, 6.164, 7.301, 12.246, 13.699, 14.684, 15.691, 19.934, 23.320}, //RPM = 1399 - { 0.551, 3.738, 4.348, 4.746, 5.906, 7.102, 12.246, 13.734, 14.895, 16.055, 20.531, 24.094}, //RPM = 1500 - { -1.289, 2.285, 3.246, 3.586, 4.605, 5.848, 12.375, 14.543, 16.148, 17.789, 23.742, 24.598}, //RPM = 1999 - { -2.203, 1.477, 2.543, 2.953, 4.090, 5.215, 12.609, 15.352, 16.898, 18.457, 24.598, 24.598}, //RPM = 2199 - - } -}; -struct AlphaStruct fuelmap_10 = { //probar este mapa - { - { 5.320, 7.289, 7.406, 7.500, 7.781, 8.355, 10.898, 11.566, 11.871, 12.188, 13.184, 14.004}, //RPM = 99 - { 5.484, 7.711, 7.852, 7.957, 8.285, 9.094, 11.531, 12.176, 12.445, 12.738, 13.699, 14.461}, //RPM = 199 - { 4.395, 6.715, 6.973, 7.254, 8.074, 9.188, 11.449, 11.977, 12.469, 12.961, 14.438, 15.609}, //RPM = 424 - { 3.984, 6.316, 6.691, 7.008, 7.898, 9.105, 11.520, 12.164, 12.727, 13.301, 15.070, 16.465}, //RPM = 499 - { 2.602, 5.332, 5.684, 6.047, 6.914, 8.063, 11.813, 12.785, 13.676, 14.520, 17.238, 19.418}, //RPM = 894 - { 2.320, 5.109, 5.531, 5.836, 6.750, 7.805, 11.906, 13.008, 13.934, 14.859, 17.719, 20.004}, //RPM = 999 - { 0.961, 4.090, 4.629, 5.051, 6.281, 7.395, 12.633, 14.133, 15.305, 16.523, 20.766, 24.164}, //RPM = 1399 - { 0.645, 3.879, 4.488, 4.887, 6.035, 7.184, 12.715, 14.273, 15.586, 16.910, 21.375, 24.598}, //RPM = 1499 - { -1.172, 2.426, 3.457, 3.750, 4.781, 5.988, 13.020, 15.258, 17.039, 18.844, 24.598, 24.598}, //RPM = 1997 - { -2.027, 1.711, 2.836, 3.199, 4.254, 5.391, 13.254, 15.938, 17.801, 19.676, 24.598, 24.598}, //RPM = 2199 - } -}; -struct AlphaStruct fuelmap_25 = { //probar este mapa - { - { 5.590, 7.652, 7.781, 7.863, 8.145, 8.613, 11.133, 11.625, 12.047, 12.457, 13.477, 14.297}, //RPM = 99 - { 5.637, 7.922, 8.074, 8.168, 8.496, 9.375, 11.707, 12.223, 12.609, 13.020, 13.969, 14.730}, //RPM = 199 - { 4.535, 6.902, 7.148, 7.453, 8.309, 9.410, 11.824, 12.398, 12.879, 13.371, 14.848, 16.020}, //RPM = 425 - { 4.113, 6.469, 6.832, 7.172, 8.145, 9.352, 11.883, 12.527, 13.137, 13.723, 15.445, 16.898}, //RPM = 499 - { 2.648, 5.391, 5.777, 6.176, 7.148, 8.191, 12.223, 13.219, 14.121, 15.059, 17.730, 19.969}, //RPM = 895 - { 2.355, 5.145, 5.566, 5.918, 6.938, 7.887, 12.328, 13.488, 14.414, 15.352, 18.199, 20.520}, //RPM = 999 - { 1.066, 4.219, 4.758, 5.191, 6.410, 7.465, 13.043, 14.543, 15.949, 17.379, 21.563, 24.598}, //RPM = 1399 - { 0.750, 3.996, 4.605, 5.016, 6.152, 7.242, 13.195, 14.824, 16.254, 17.695, 22.160, 24.598}, //RPM = 1499 - { -1.043, 2.637, 3.609, 3.973, 4.922, 6.094, 13.664, 15.902, 17.859, 19.852, 24.598, 24.598}, //RPM = 2000 - { -1.875, 1.945, 3.070, 3.445, 4.430, 5.578, 13.793, 16.465, 18.668, 20.824, 24.598, 24.598}, //RPM = 2199664, 2.180, 3.398, 3.715, 4.711, 5.777, 14.461, 17.086, 19.605, 22.125, 24.598, 24.598}, //RPM = 2199 - } -}; -struct AlphaStruct fuelmap_60 = { //probar este mapa - { - { 5.918, 8.109, 8.227, 8.320, 8.590, 8.930, 11.426, 11.695, 12.246, 12.820, 13.816, 14.625}, //RPM = 99 - { 5.824, 8.180, 8.309, 8.426, 8.742, 9.727, 11.953, 12.281, 12.809, 13.336, 14.320, 15.070}, //RPM = 199 - { 4.688, 7.125, 7.359, 7.676, 8.625, 9.703, 12.293, 12.891, 13.395, 13.898, 15.375, 16.559}, //RPM = 424 - { 4.242, 6.668, 7.020, 7.395, 8.438, 9.633, 12.316, 12.949, 13.605, 14.250, 15.984, 17.414}, //RPM = 499 - { 2.766, 5.438, 5.906, 6.246, 7.395, 8.262, 12.715, 13.781, 14.766, 15.656, 18.387, 20.590}, //RPM = 895 - { 2.391, 5.203, 5.625, 6.000, 7.102, 8.004, 12.879, 14.074, 15.023, 15.996, 18.844, 21.117}, //RPM = 999 - { 1.195, 4.395, 4.945, 5.355, 6.563, 7.570, 13.523, 15.094, 16.723, 18.398, 22.605, 24.598}, //RPM = 1399 - { 0.867, 4.172, 4.781, 5.168, 6.328, 7.359, 13.734, 15.480, 17.133, 18.738, 23.215, 24.598}, //RPM = 1499 - { -0.891, 2.824, 3.855, 4.184, 5.133, 6.246, 14.449, 16.793, 18.949, 21.035, 24.598, 24.598}, //RPM = 1999 - { -1.641, 2.227, 3.398, 3.738, 4.664, 5.789, 14.543, 17.180, 19.688, 22.207, 24.598, 24.598}, //RPM = 2199 - } -}; -struct fuelMapIndexes fuelMapI = { //probar este mapa - { 100, 200, 425, 500, 895, 1000, 1400, 1500, 2000, 2200}, //N_RPM = 17 - { 0.375, 0.500, 1.625, 2.500, 5.000, 10.000, 30.000, 35.000, 40.000, 45.000, 60.000, 72.000}, - { -15, 3, 25, 46, 75} -}; -#elif defined(_004002) -struct AlphaStruct fuelmap_m12 = { //probar este mapa - { - { 5.367, 7.371, 7.453, 7.465, 7.512, 8.414, 10.570, 11.320, 11.625, 11.930, 13.383, 14.543}, //RPM = 99 - { 5.133, 7.242, 7.746, 7.898, 8.344, 9.059, 11.191, 11.836, 12.234, 12.621, 13.852, 14.414}, //RPM = 199 - { 5.332, 7.313, 7.395, 7.406, 7.465, 8.379, 10.535, 11.309, 11.602, 11.883, 13.336, 14.496}, //RPM = 99 - { 5.098, 7.219, 7.395, 7.559, 7.992, 9.023, 11.168, 11.754, 11.953, 12.164, 13.395, 14.379}, //RPM = 199 - { 4.125, 6.340, 6.656, 7.008, 7.992, 9.059, 10.898, 11.426, 11.930, 12.434, 13.957, 15.188}, //RPM = 425 - { 3.762, 6.023, 6.445, 6.832, 7.969, 8.859, 11.063, 11.660, 12.164, 12.691, 14.461, 15.879}, //RPM = 499 - { 2.320, 4.898, 5.754, 6.035, 7.031, 8.098, 11.402, 12.270, 13.172, 14.074, 17.039, 19.383}, //RPM = 896 - { 1.980, 4.664, 5.590, 5.883, 6.738, 7.910, 11.414, 12.410, 13.441, 14.496, 17.742, 20.344}, //RPM = 999 - { 0.539, 3.516, 4.430, 4.816, 5.941, 7.301, 12.047, 13.535, 14.402, 15.281, 19.418, 22.699}, //RPM = 1399 - { 0.188, 3.246, 4.172, 4.582, 5.813, 7.184, 12.141, 13.629, 14.695, 15.762, 20.074, 23.566}, //RPM = 1499 - { -1.840, 1.547, 2.320, 2.953, 4.723, 6.293, 12.410, 14.320, 15.914, 17.496, 23.484, 24.598}, //RPM = 1999 - { -2.824, 0.633, 1.277, 2.004, 4.102, 5.707, 12.586, 14.859, 16.336, 17.836, 24.598, 24.598}, //RPM = 2199 - } -}; -struct AlphaStruct fuelmap_m5 = { //probar este mapa - { - { 5.461, 7.488, 7.570, 7.570, 7.641, 8.496, 10.641, 11.344, 11.684, 12.023, 13.477, 14.625}, //RPM = 99 - { 5.168, 7.313, 7.500, 7.641, 8.086, 9.152, 11.250, 11.777, 12.035, 12.281, 13.512, 14.496}, //RPM = 199 - { 4.195, 6.410, 6.703, 7.090, 8.098, 9.176, 11.051, 11.613, 12.094, 12.621, 14.156, 15.375}, //RPM = 424 - { 3.809, 6.059, 6.516, 6.902, 8.051, 8.953, 11.203, 11.789, 12.340, 12.891, 14.602, 16.043}, //RPM = 499 - { 2.320, 4.922, 5.801, 6.117, 7.113, 8.121, 11.602, 12.480, 13.383, 14.309, 17.250, 19.664}, //RPM = 896 - { 2.016, 4.676, 5.613, 5.918, 6.797, 7.934, 11.602, 12.621, 13.664, 14.719, 17.965, 20.555}, //RPM = 999 - { 0.586, 3.586, 4.500, 4.887, 6.000, 7.336, 12.223, 13.734, 14.684, 15.645, 19.781, 23.074}, //RPM = 1399 - { 0.234, 3.316, 4.242, 4.641, 5.859, 7.230, 12.328, 13.875, 15.012, 16.137, 20.461, 23.918}, //RPM = 1499 - { -1.781, 1.617, 2.402, 3.035, 4.805, 6.352, 12.703, 14.637, 16.289, 17.941, 23.953, 24.598}, //RPM = 1999 - { -2.742, 0.738, 1.406, 2.109, 4.184, 5.777, 12.832, 15.094, 16.699, 18.352, 24.598, 24.598}, //RPM = 2199 - } -}; -struct AlphaStruct fuelmap_10 = { //probar este mapa - { - { 5.578, 7.641, 7.723, 7.734, 7.793, 8.602, 10.746, 11.367, 11.754, 12.141, 13.594, 14.754}, //RPM = 99 - { 5.238, 7.395, 7.582, 7.734, 8.180, 9.281, 11.332, 11.801, 12.105, 12.410, 13.641, 14.625}, //RPM = 199 - { 4.242, 6.504, 6.820, 7.172, 8.227, 9.281, 11.215, 11.789, 12.270, 12.820, 14.332, 15.563}, //RPM = 425 - { 3.867, 6.152, 6.586, 6.996, 8.180, 9.070, 11.355, 11.977, 12.516, 13.090, 14.848, 16.254}, //RPM = 499 - { 2.367, 4.957, 5.801, 6.164, 7.184, 8.203, 11.766, 12.645, 13.594, 14.508, 17.496, 19.828}, //RPM = 895 - { 2.016, 4.699, 5.625, 5.965, 6.891, 7.992, 11.813, 12.832, 13.887, 14.941, 18.199, 20.813}, //RPM = 999 - { 0.633, 3.668, 4.559, 4.945, 6.047, 7.371, 12.410, 13.934, 14.977, 16.031, 20.168, 23.461}, //RPM = 1399 - { 0.281, 3.375, 4.289, 4.699, 5.941, 7.254, 12.539, 14.109, 15.316, 16.512, 20.848, 24.305}, //RPM = 1499 - { -1.723, 1.699, 2.508, 3.117, 4.887, 6.410, 12.996, 14.965, 16.688, 18.410, 24.422, 24.598}, //RPM = 1999 - { -2.672, 0.832, 1.523, 2.227, 4.266, 5.871, 13.125, 15.340, 17.133, 18.902, 24.598, 24.598}, //RPM = 2199 - } -}; -struct AlphaStruct fuelmap_25 = { //probar este mapa - { - { 5.719, 7.828, 7.910, 7.922, 7.969, 8.742, 10.863, 11.391, 11.848, 12.293, 13.746, 14.906}, //RPM = 99 - { 5.320, 7.512, 7.699, 7.852, 8.297, 9.434, 11.438, 11.824, 12.199, 12.574, 13.793, 14.777}, //RPM = 199 - { 4.313, 6.598, 6.914, 7.289, 8.379, 9.387, 11.461, 12.047, 12.551, 13.066, 14.590, 15.820}, //RPM = 425 - { 3.926, 6.258, 6.680, 7.102, 8.320, 9.211, 11.566, 12.211, 12.785, 13.336, 15.094, 16.523}, //RPM = 499 - { 2.414, 5.004, 5.871, 6.199, 7.289, 8.320, 12.035, 12.984, 13.934, 14.906, 17.824, 20.215}, //RPM = 896 - { 2.051, 4.734, 5.660, 6.000, 6.996, 8.051, 12.117, 13.184, 14.227, 15.316, 18.563, 21.164}, //RPM = 999 - { 0.703, 3.773, 4.664, 5.039, 6.141, 7.430, 12.668, 14.250, 15.445, 16.652, 20.777, 24.082}, //RPM = 1399 - { 0.352, 3.457, 4.395, 4.793, 5.988, 7.324, 12.891, 14.531, 15.832, 17.156, 21.469, 24.598}, //RPM = 1499 - { -1.629, 1.816, 2.648, 3.258, 5.004, 6.504, 13.477, 15.504, 17.344, 19.195, 24.598, 24.598}, //RPM = 1999 - { -2.531, 1.020, 1.734, 2.414, 4.406, 6.000, 13.582, 15.773, 17.801, 19.840, 24.598, 24.598}, //RPM = 2199 - } -}; -struct AlphaStruct fuelmap_60 = { //probar este mapa - { - { 5.977, 8.180, 8.262, 8.273, 8.320, 8.988, 11.098, 11.449, 12.000, 12.563, 14.016, 15.176}, //RPM = 99 - { 5.473, 7.711, 7.887, 8.039, 8.484, 9.715, 11.613, 11.871, 12.352, 12.832, 14.063, 15.047}, //RPM = 199 - { 4.441, 6.773, 7.078, 7.477, 8.602, 9.645, 11.813, 12.434, 12.938, 13.453, 14.965, 16.195}, //RPM = 424 - { 4.043, 6.387, 6.809, 7.277, 8.578, 9.445, 11.895, 12.563, 13.137, 13.734, 15.480, 16.922}, //RPM = 499 - { 2.449, 5.063, 5.918, 6.340, 7.500, 8.391, 12.363, 13.359, 14.332, 15.363, 18.281, 20.684}, //RPM = 895 - { 2.063, 4.770, 5.695, 6.070, 7.125, 8.133, 12.527, 13.617, 14.695, 15.797, 19.043, 21.645}, //RPM = 999 - { 0.809, 3.879, 4.793, 5.168, 6.258, 7.512, 13.066, 14.648, 16.031, 17.438, 21.504, 24.598}, //RPM = 1399 - { 0.445, 3.598, 4.523, 4.922, 6.141, 7.395, 13.324, 15.023, 16.453, 17.918, 22.254, 24.598}, //RPM = 1499 - { -1.535, 1.969, 2.836, 3.422, 5.145, 6.633, 14.051, 16.125, 18.129, 20.098, 24.598, 24.598}, //RPM = 1999 - { -2.379, 1.230, 1.969, 2.625, 4.547, 6.164, 14.121, 16.277, 18.586, 20.883, 24.598, 24.598}, //RPM = 2199 - } -}; -struct AlphaStruct fuelmap_80 = { //probar este mapa - { - { 6.234, 8.531, 8.602, 8.613, 8.672, 9.234, 11.320, 11.508, 12.176, 12.844, 14.285, 15.445}, //RPM = 99 - { 5.625, 7.910, 8.098, 8.238, 8.684, 9.996, 11.801, 11.918, 12.516, 13.102, 14.332, 15.316}, //RPM = 199 - { 4.582, 6.949, 7.266, 7.664, 8.859, 9.891, 12.188, 12.844, 13.348, 13.840, 15.387, 16.594}, //RPM = 424 - { 4.160, 6.527, 6.973, 7.441, 8.801, 9.668, 12.234, 12.879, 13.523, 14.203, 15.973, 17.367}, //RPM = 499 - { 2.496, 5.145, 5.988, 6.434, 7.676, 8.520, 12.832, 13.840, 14.859, 15.891, 18.855, 21.188}, //RPM = 895 - { 2.109, 4.805, 5.742, 6.141, 7.313, 8.238, 12.984, 14.109, 15.223, 16.336, 19.582, 22.172}, //RPM = 999 - { 0.914, 4.031, 4.945, 5.320, 6.387, 7.594, 13.488, 15.117, 16.723, 18.328, 22.465, 24.598}, //RPM = 1399 - { 0.551, 3.727, 4.652, 5.074, 6.281, 7.488, 13.816, 15.598, 17.203, 18.820, 23.145, 24.598}, //RPM = 1499 - { -1.383, 2.156, 3.023, 3.621, 5.320, 6.773, 14.754, 16.922, 19.066, 21.223, 24.598, 24.598}, //RPM = 1999 - { -2.191, 1.477, 2.262, 2.906, 4.746, 6.363, 14.766, 16.898, 19.523, 22.160, 24.598, 24.598}, //RPM = 2199 - } -}; - -struct fuelMapIndexes fuelMapI = { //probar este mapa - { 100, 200, 425, 500, 950, 1000, 1400, 1500, 2000, 2200}, //N_RPM = 17 - { 0.375, 0.500, 1.625, 2.500, 5.000, 10.000, 30.000, 35.000, 40.000, 45.000, 60.000, 72.000}, - { -15.5, -5.5 ,6.3, 27.5, 47.8, 75} -}; -#elif defined(_504010) -struct AlphaStruct fuelmap_m12 = { //probar este mapa - { - { 8.594, 9.368, 9.344, 9.473, 9.801, 10.083, 11.219, 11.747, 12.579, 13.141, 13.961, 15.919}, //RPM = 99 - { 7.841, 8.626, 8.509, 8.638, 9.001, 9.388, 11.087, 11.509, 12.165, 12.716, 13.536, 15.505}, //RPM = 199 - { 6.874, 7.741, 7.659, 7.870, 8.456, 9.065, 10.846, 11.631, 12.862, 13.940, 15.229, 18.182}, //RPM = 399 - { 6.519, 7.444, 7.409, 7.644, 8.230, 8.816, 10.784, 11.792, 13.269, 14.487, 15.601, 17.980}, //RPM = 499 - { 4.721, 5.893, 6.127, 6.397, 7.018, 7.498, 10.604, 12.608, 15.127, 17.272, 18.104, 19.123}, //RPM = 999 - { 2.724, 4.166, 4.646, 4.916, 5.619, 6.369, 10.740, 13.154, 16.986, 18.931, 19.634, 20.419}, //RPM = 1499 - { 1.550, 3.085, 3.694, 3.976, 4.726, 5.675, 10.714, 13.222, 17.968, 19.409, 20.124, 21.237}, //RPM = 1749 - { 0.501, 2.189, 2.411, 2.704, 3.525, 4.661, 10.052, 13.228, 19.228, 19.884, 20.048, 20.048}, //RPM = 2099 - } -}; -struct AlphaStruct fuelmap_m5 = { //probar este mapa - { - { 8.594, 9.379, 9.497, 9.626, 9.954, 10.235, 11.348, 11.958, 12.860, 13.422, 14.243, 16.200}, //RPM = 99 - { 7.841, 8.638, 8.650, 8.790, 9.154, 9.540, 11.216, 11.720, 12.458, 13.009, 13.818, 15.786}, //RPM = 199 - { 6.874, 7.752, 7.799, 8.022, 8.608, 9.217, 10.975, 11.842, 13.143, 14.233, 15.522, 18.475}, //RPM = 399 - { 6.519, 7.444, 7.538, 7.761, 8.347, 8.933, 10.925, 11.991, 13.573, 14.804, 15.894, 18.273}, //RPM = 499 - { 4.721, 5.905, 6.198, 6.455, 7.088, 7.580, 10.768, 12.830, 15.549, 17.694, 18.526, 19.510}, //RPM = 999 - { 2.876, 4.330, 4.810, 5.080, 5.783, 6.533, 10.974, 13.541, 17.595, 19.541, 20.255, 21.041}, //RPM = 1499 - { 1.773, 3.343, 3.941, 4.222, 4.972, 5.921, 11.019, 13.737, 18.659, 20.112, 20.827, 21.941}, //RPM = 1749 - { 0.701, 2.411, 2.646, 2.927, 3.759, 4.896, 10.568, 13.872, 20.177, 20.845, 20.986, 21.115}, //RPM = 2099 - } -}; -struct AlphaStruct fuelmap_10 = { //probar este mapa - { - { 8.594, 9.391, 9.590, 9.719, 10.047, 10.340, 11.430, 12.086, 13.047, 13.610, 14.430, 16.387}, //RPM = 99 - { 7.841, 8.650, 8.755, 8.896, 9.259, 9.646, 11.310, 11.861, 12.657, 13.208, 14.029, 15.997}, //RPM = 199 - { 6.874, 7.764, 7.916, 8.139, 8.725, 9.334, 11.081, 12.006, 13.366, 14.456, 15.745, 18.698}, //RPM = 399 - { 6.519, 7.433, 7.632, 7.866, 8.452, 9.038, 11.030, 12.155, 13.819, 15.038, 16.151, 18.530}, //RPM = 499 - { 4.721, 5.905, 6.244, 6.502, 7.135, 7.627, 10.909, 13.018, 15.924, 18.069, 18.901, 19.920}, //RPM = 999 - { 3.017, 4.470, 4.951, 5.232, 5.935, 6.685, 11.197, 13.892, 18.169, 20.115, 20.818, 21.615}, //RPM = 1499 - { 1.995, 3.577, 4.175, 4.456, 5.206, 6.155, 11.312, 14.241, 19.327, 20.780, 21.495, 22.620}, //RPM = 1749 - { 0.900, 2.634, 2.857, 3.232, 3.970, 5.130, 11.072, 14.505, 21.126, 21.783, 21.947, 21.935}, //RPM = 2099 - } -}; -struct AlphaStruct fuelmap_25 = { //probar este mapa - { - { 8.583, 9.415, 9.825, 9.965, 10.294, 10.575, 11.641, 12.415, 13.493, 14.055, 14.876, 16.833}, //RPM = 99 - { 7.829, 8.661, 8.978, 9.118, 9.482, 9.868, 11.497, 12.165, 13.079, 13.630, 14.450, 16.419}, //RPM = 199 - { 6.862, 7.776, 8.127, 8.350, 8.936, 9.545, 11.268, 12.288, 13.764, 14.854, 16.143, 19.096}, //RPM = 399 - { 6.507, 7.468, 7.796, 8.030, 8.616, 9.202, 11.218, 12.425, 14.206, 15.425, 16.538, 18.917}, //RPM = 499 - { 4.709, 5.916, 6.326, 6.584, 7.217, 7.709, 11.108, 13.299, 16.451, 18.596, 19.428, 20.448}, //RPM = 999 - { 3.193, 4.669, 5.162, 5.431, 6.134, 6.884, 11.490, 14.349, 18.896, 20.841, 21.544, 22.330}, //RPM = 1499 - { 2.265, 3.870, 4.468, 4.749, 5.499, 6.448, 11.675, 14.839, 20.124, 21.577, 22.292, 23.405}, //RPM = 1749 - { 1.146, 2.892, 3.115, 3.396, 4.228, 5.376, 11.622, 15.267, 22.169, 22.826, 22.978, 22.978}, //RPM = 2099 - } -}; -struct AlphaStruct fuelmap_60 = { //probar este mapa - { - { 8.583, 9.426, 10.024, 10.176, 10.504, 10.786, 11.817, 12.262, 13.844, 14.442, 14.676, 17.219}, //RPM = 99 - { 7.829, 8.685, 9.177, 9.329, 9.693, 10.079, 11.685, 12.446, 13.466, 14.017, 14.837, 16.806}, //RPM = 199 - { 6.862, 7.788, 8.327, 8.561, 9.147, 9.756, 11.444, 12.569, 14.151, 15.241, 16.530, 19.483}, //RPM = 399 - { 6.507, 7.480, 7.960, 8.194, 8.792, 9.378, 11.394, 12.706, 14.605, 15.823, 16.937, 19.292}, //RPM = 499 - { 4.709, 5.916, 6.420, 6.678, 7.311, 7.803, 11.319, 13.592, 17.026, 19.170, 20.002, 21.022}, //RPM = 999 - { 3.404, 4.892, 5.373, 5.642, 6.345, 7.095, 11.818, 14.865, 19.716, 21.662, 22.365, 23.162}, //RPM = 1499 - { 2.569, 4.198, 4.796, 5.089, 5.839, 6.788, 12.097, 15.530, 21.073, 22.515, 23.230, 24.343}, //RPM = 1749 - { 1.427, 3.197, 3.431, 3.701, 4.533, 5.681, 12.326, 16.087, 23.447, 24.091, 24.255, 24.267}, //RPM = 2099 - } -}; - -struct fuelMapIndexes fuelMapI = { //probar este mapa - { 100, 200, 400, 500, 1000, 1500, 1750, 2100}, //N_RPM = 17 - { 0.375, 0.500, 1.625, 2.500, 5.000, 8.313, 24.938, 35.000, 49.813, 60.000, 72.000, 99.625}, - { -15, 5 , 30, 45.5, 72.5} -}; -#elif defined(_504003) -struct AlphaStruct fuelmap_m12 = { //probar este mapa - { - { 0.563, 10.008, 10.113, 10.172, 10.676, 11.895, 12.691, 13.406, 14.273, 21.059}, //RPM = 99.75 - { 0.609, 10.324, 10.559, 10.688, 11.531, 12.668, 13.535, 13.910, 14.730, 20.730}, //RPM = 199.75 - { 0.527, 11.098, 11.438, 11.672, 13.078, 14.602, 15.680, 16.289, 17.742, 18.680}, //RPM = 399.75 - { 1.031, 14.977, 15.410, 15.727, 17.215, 20.238, 22.348, 23.801, 26.484, 28.441}, //RPM = 999.75 - { 0.984, 19.594, 20.016, 20.262, 22.066, 27.398, 30.633, 33.188, 33.188, 33.188}, //RPM = 1749.75 - { 1.066, 22.266, 22.863, 23.074, 25.090, 30.961, 34.816, 38.145, 38.109, 38.121}, //RPM = 2149.50 - } -}; -struct AlphaStruct fuelmap_m5 = { //probar este mapa - { - { 0.656, 10.102, 10.207, 10.266, 10.758, 12.023, 12.867, 13.570, 14.438, 21.223}, //RPM = 99.75 - { 0.691, 10.406, 10.641, 10.758, 11.613, 12.949, 13.781, 14.168, 14.965, 20.977}, //RPM = 199.75 - { 0.656, 11.227, 11.578, 11.801, 13.219, 14.801, 15.926, 16.629, 18.082, 19.031}, //RPM = 399.75 - { 1.078, 15.023, 15.457, 15.773, 17.262, 20.402, 22.711, 24.246, 26.930, 28.887}, //RPM = 999.75 - { 1.172, 19.781, 20.215, 20.461, 22.266, 27.703, 31.125, 33.926, 33.961, 33.949}, //RPM = 1749.75 - { 1.313, 22.500, 23.098, 23.309, 25.336, 31.313, 35.449, 39.105, 39.070, 39.094}, //RPM = 2149.75 - } -}; -struct AlphaStruct fuelmap_10 = { //probar este mapa - { - { 0.762, 10.207, 10.313, 10.371, 10.863, 12.164, 13.078, 13.770, 14.637, 21.410}, //RPM = 99.75 - { 0.773, 10.500, 10.723, 10.863, 11.695, 13.301, 14.086, 14.461, 15.270, 21.281}, //RPM = 199.75 - { 0.832, 11.402, 11.754, 11.977, 13.395, 15.047, 16.242, 17.039, 18.492, 19.441}, //RPM = 399.75 - { 1.137, 15.082, 15.516, 15.832, 17.320, 20.602, 23.133, 24.785, 27.492, 29.461}, //RPM = 999.75 - { 1.430, 20.039, 20.438, 20.707, 22.488, 28.078, 31.734, 34.898, 34.922, 34.793}, //RPM = 1749.75 - { 1.605, 22.793, 23.391, 23.602, 25.629, 31.781, 36.211, 40.277, 40.289, 40.289}, //RPM = 2149.75 - } -}; -struct AlphaStruct fuelmap_25 = { //probar este mapa - { - { 0.844, 10.289, 10.395, 10.453, 10.945, 12.293, 13.242, 13.922, 14.789, 21.574}, //RPM = 99.75 - { 0.855, 10.570, 10.816, 10.934, 11.777, 13.559, 14.309, 14.695, 15.504, 21.516}, //RPM = 199.75 - { 0.961, 11.531, 11.883, 12.105, 13.512, 15.234, 16.500, 17.367, 18.820, 19.770}, //RPM = 399.75 - { 1.172, 15.117, 15.563, 15.867, 17.355, 20.754, 23.496, 25.219, 27.902, 29.859}, //RPM = 999.75 - { 1.617, 20.215, 20.648, 20.883, 22.688, 28.348, 32.203, 35.555, 35.555, 35.566}, //RPM = 1749.75 - { 1.828, 23.004, 23.613, 23.824, 25.852, 32.133, 36.773, 41.203, 41.227, 41.215}, //RPM = 2149.75 - } -}; -struct AlphaStruct fuelmap_60 = { //probar este mapa - { - { 0.938, 10.383, 10.488, 10.547, 11.039, 12.410, 13.418, 14.086, 14.953, 21.738}, //RPM = 99.75 - { 0.949, 10.664, 10.898, 11.027, 11.871, 13.863, 14.578, 14.965, 15.773, 21.773}, //RPM = 199.75 - { 1.113, 11.684, 12.035, 12.258, 13.664, 15.457, 16.770, 17.730, 19.195, 20.145}, //RPM = 399.75 - { 1.230, 15.176, 15.609, 15.926, 17.414, 20.930, 23.895, 25.711, 28.395, 30.352}, //RPM = 999.75 - { 1.828, 20.426, 20.848, 21.094, 22.910, 28.699, 32.754, 36.363, 36.387, 36.398}, //RPM = 1749.75 - { 2.098, 23.273, 23.883, 24.094, 26.121, 32.543, 37.512, 42.293, 42.305, 42.316}, //RPM = 2149.75 - } -}; -/*struct AlphaStruct fuelmap_80 = { //probar este mapa - { - { 6.234, 8.531, 8.602, 8.613, 8.672, 9.234, 11.320, 11.508, 12.176, 12.844, 14.285, 15.445}, //RPM = 99 - { 5.625, 7.910, 8.098, 8.238, 8.684, 9.996, 11.801, 11.918, 12.516, 13.102, 14.332, 15.316}, //RPM = 199 - { 4.582, 6.949, 7.266, 7.664, 8.859, 9.891, 12.188, 12.844, 13.348, 13.840, 15.387, 16.594}, //RPM = 424 - { 4.160, 6.527, 6.973, 7.441, 8.801, 9.668, 12.234, 12.879, 13.523, 14.203, 15.973, 17.367}, //RPM = 499 - { 2.496, 5.145, 5.988, 6.434, 7.676, 8.520, 12.832, 13.840, 14.859, 15.891, 18.855, 21.188}, //RPM = 895 - { 2.109, 4.805, 5.742, 6.141, 7.313, 8.238, 12.984, 14.109, 15.223, 16.336, 19.582, 22.172}, //RPM = 999 - { 0.914, 4.031, 4.945, 5.320, 6.387, 7.594, 13.488, 15.117, 16.723, 18.328, 22.465, 24.598}, //RPM = 1399 - { 0.551, 3.727, 4.652, 5.074, 6.281, 7.488, 13.816, 15.598, 17.203, 18.820, 23.145, 24.598}, //RPM = 1499 - { -1.383, 2.156, 3.023, 3.621, 5.320, 6.773, 14.754, 16.922, 19.066, 21.223, 24.598, 24.598}, //RPM = 1999 - { -2.191, 1.477, 2.262, 2.906, 4.746, 6.363, 14.766, 16.898, 19.523, 22.160, 24.598, 24.598}, //RPM = 2199 - } -};*/ - -struct fuelMapIndexes fuelMapI = { //probar este mapa - { 100, 200, 400, 1000, 1750, 2150}, //N_RPM = 17 - { 0.031, 0.406, 1.656, 2.406, 8.313, 24.906, 37.313, 45.688, 60.000, 72.000}, - { -15.0, 5.1 ,30, 50, 74.5} -}; -#elif defined(_504009) -struct AlphaStruct fuelmap_m12 = { //probar este mapa - { - { 0.563, 9.387, 9.680, 9.785, 10.406, 11.742, 12.434, 12.844, 13.898, 20.602}, //RPM = 99.75 - { 0.598, 9.785, 9.820, 9.996, 11.063, 12.762, 13.020, 13.242, 14.180, 20.145}, //RPM = 199.75 - { 0.680, 10.910, 11.227, 11.426, 12.867, 14.355, 14.988, 15.516, 16.758, 17.613}, //RPM = 399.75 - { 0.949, 14.777, 15.164, 15.410, 16.605, 19.207, 21.059, 22.219, 24.609, 26.367}, //RPM = 999.50 - { 1.137, 19.277, 19.629, 19.875, 21.234, 25.582, 28.324, 30.281, 34.922, 37.922}, //RPM = 1749.75 - { 1.336, 22.617, 23.496, 23.801, 25.523, 29.719, 32.742, 34.676, 40.770, 42.176}, //RPM = 2149.75 - } -}; -struct AlphaStruct fuelmap_m5 = { //probar este mapa - { - { 0.633, 9.457, 9.750, 9.855, 10.477, 11.848, 12.645, 13.148, 14.203, 20.906}, //RPM = 99.75 - { 0.668, 9.855, 9.891, 10.066, 11.133, 12.855, 13.230, 13.547, 14.484, 20.438}, //RPM = 199.75 - { 0.738, 10.969, 11.285, 11.484, 12.926, 14.461, 15.199, 15.809, 17.051, 17.906}, //RPM = 399.75 - { 1.020, 14.848, 15.234, 15.480, 16.676, 19.406, 21.352, 22.641, 25.031, 26.801}, //RPM = 999.75 - { 1.266, 19.406, 19.758, 20.004, 21.363, 25.840, 28.734, 30.902, 35.543, 38.543}, //RPM = 1749.75 - { 1.488, 22.770, 23.672, 23.953, 25.652, 29.988, 33.117, 35.473, 41.555, 42.984}, //RPM = 2149.75 - } -}; -struct AlphaStruct fuelmap_10 = { //probar este mapa - { - { 0.715, 9.539, 9.832, 9.938, 10.559, 11.965, 12.902, 13.512, 14.566, 21.270}, //RPM = 99.75 - { 0.750, 9.938, 9.961, 10.148, 11.203, 12.984, 13.488, 13.898, 14.836, 20.801}, //RPM = 199.75 - { 0.820, 11.051, 11.367, 11.566, 13.008, 14.578, 15.457, 16.172, 17.414, 18.270}, //RPM = 399.75 - { 1.113, 14.941, 15.328, 15.574, 16.770, 19.641, 21.703, 23.156, 25.547, 27.316}, //RPM = 999.75 - { 1.430, 19.570, 19.922, 20.168, 21.527, 26.168, 29.238, 31.688, 36.316, 39.316}, //RPM = 1749.75 - { 1.676, 22.934, 23.859, 24.117, 25.840, 30.328, 33.609, 36.457, 42.563, 43.969}, //RPM = 2149.75 - } -}; -struct AlphaStruct fuelmap_25 = { //probar este mapa - { - { 0.773, 9.598, 9.891, 9.996, 10.617, 12.059, 13.102, 13.793, 14.836, 21.539}, //RPM = 99.75 - { 0.809, 9.996, 10.031, 10.207, 11.273, 13.078, 13.676, 14.180, 15.129, 21.082}, //RPM = 199.75 - { 0.879, 11.109, 11.426, 11.625, 13.066, 14.672, 15.656, 16.453, 17.695, 18.551}, //RPM = 399.75 - { 1.184, 15.012, 15.398, 15.645, 16.840, 19.828, 21.984, 23.555, 25.945, 27.715}, //RPM = 999.75 - { 1.547, 19.688, 20.039, 20.285, 21.645, 26.414, 29.613, 32.262, 36.914, 39.902}, //RPM = 1749.75 - { 1.805, 23.086, 23.988, 24.328, 25.980, 30.598, 33.973, 37.207, 43.301, 44.707}, //RPM = 2149.75 - } -}; -struct AlphaStruct fuelmap_60 = { //probar este mapa - { - { 0.855, 9.680, 9.973, 10.078, 10.699, 12.176, 13.348, 14.145, 15.188, 21.891}, //RPM = 99.75 - { 0.879, 10.066, 10.102, 10.277, 11.344, 13.195, 13.934, 14.531, 15.469, 21.434}, //RPM = 199.75 - { 0.949, 11.191, 11.508, 11.707, 13.148, 14.789, 15.902, 16.781, 18.047, 18.902}, //RPM = 399.50 - { 1.277, 15.105, 15.492, 15.738, 16.934, 20.063, 22.324, 24.047, 26.438, 28.207}, //RPM = 999.75 - { 1.699, 19.840, 20.191, 20.438, 21.797, 26.730, 30.094, 33.000, 37.664, 40.652}, //RPM = 1749.75 - { 1.992, 23.262, 24.164, 24.457, 26.145, 30.914, 34.418, 38.227, 44.238, 45.680}, //RPM = 2150.00 - } -}; - - -struct fuelMapIndexes fuelMapI = { //probar este mapa - { 100, 200, 400, 1000, 1750, 2150}, //N_RPM = 17 - { 0.031, 0.406, 1.656, 2.406, 8.313, 24.906, 37.313, 45.688, 60.000, 72.000}, - { -15.0, 5.1 ,30, 50, 74.5} -}; -#elif defined(_504012) -struct AlphaStruct fuelmap_m12 = { //probar este mapa - { - { 0.434, 9.152, 9.352, 9.469, 10.430, 10.945, 11.402, 12.246, 13.137, 13.793, 13.887}, //RPM = 99.75 - { 0.480, 9.668, 10.395, 10.465, 11.027, 12.023, 12.598, 13.852, 14.895, 15.867, 15.973}, //RPM = 199.75 - { 0.539, 10.969, 11.215, 11.367, 12.504, 14.109, 14.309, 14.625, 15.797, 16.641, 16.652}, //RPM = 399.75 - { 0.949, 14.871, 15.281, 15.492, 17.109, 18.656, 19.605, 21.832, 23.906, 25.641, 25.641}, //RPM = 999.75 - { 1.441, 19.441, 19.781, 19.945, 21.434, 23.508, 24.961, 27.926, 31.313, 31.887, 31.898}, //RPM = 1599.75 - { 1.559, 21.316, 21.855, 21.984, 23.309, 26.672, 28.266, 31.793, 34.453, 34.465, 34.488}, //RPM = 1999.75 - } -}; -struct AlphaStruct fuelmap_m5 = { //probar este mapa - { - { 0.586, 9.305, 9.504, 9.621, 10.582, 11.203, 11.707, 12.668, 13.535, 14.191, 14.273}, //RPM = 99.75 - { 0.621, 9.809, 10.535, 10.605, 11.168, 12.246, 12.891, 14.191, 15.328, 16.277, 16.383}, //RPM = 199.75 - { 0.691, 11.121, 11.355, 11.508, 12.645, 14.344, 14.766, 15.480, 16.652, 17.508, 17.496}, //RPM = 399.75 - { 1.031, 14.953, 15.363, 15.574, 17.191, 18.797, 19.887, 22.254, 24.457, 26.180, 26.191}, //RPM = 999.75 - { 1.453, 19.453, 19.793, 19.957, 21.445, 23.777, 25.254, 28.500, 32.039, 32.625, 32.625}, //RPM = 1599.75 - { 1.582, 21.316, 21.879, 22.008, 23.320, 26.953, 28.688, 32.496, 35.309, 35.297, 35.320}, //RPM = 1999.75 - } -}; -struct AlphaStruct fuelmap_10 = { //probar este mapa - { - { 0.738, 9.457, 9.656, 9.773, 10.723, 11.461, 12.000, 13.090, 13.934, 14.590, 14.684}, //RPM = 99.75 - { 0.762, 9.949, 10.676, 10.746, 11.309, 12.469, 13.195, 14.531, 15.750, 16.711, 16.816}, //RPM = 199.75 - { 0.844, 11.273, 11.520, 11.672, 12.797, 14.602, 15.246, 16.406, 17.578, 18.434, 18.422}, //RPM = 399.75 - { 1.113, 15.035, 15.445, 15.656, 17.273, 18.949, 20.191, 22.734, 25.043, 26.777, 26.777}, //RPM = 999.75 - { 1.477, 19.477, 19.816, 19.980, 21.469, 24.070, 25.594, 29.109, 32.848, 33.422, 33.434}, //RPM = 1599.75 - { 1.617, 21.387, 21.914, 22.031, 23.367, 27.258, 29.145, 33.293, 36.258, 36.270, 36.281}, //RPM = 1999.75 - } -}; -struct AlphaStruct fuelmap_25 = { //probar este mapa - { - { 0.855, 9.574, 9.773, 9.891, 10.852, 11.684, 12.234, 13.441, 14.262, 14.918, 15.012}, //RPM = 99.75 - { 0.879, 10.066, 10.793, 10.863, 11.426, 12.645, 13.441, 14.824, 16.102, 17.063, 17.168}, //RPM = 199.75 - { 0.973, 11.402, 11.648, 11.789, 12.926, 14.813, 15.645, 17.156, 18.328, 19.184, 19.172}, //RPM = 399.75 - { 1.184, 15.105, 15.516, 15.727, 17.344, 19.078, 20.449, 23.121, 25.535, 27.270, 27.246}, //RPM = 999.25 - { 1.500, 19.500, 19.840, 20.004, 21.492, 24.316, 25.875, 29.625, 33.516, 34.102, 34.102}, //RPM = 1599.75 - { 1.629, 21.410, 21.926, 22.043, 23.379, 27.516, 29.520, 33.949, 37.055, 37.066, 37.066}, //RPM = 1999.75 - } -}; -struct AlphaStruct fuelmap_60 = { //probar este mapa - { - { 1.020, 9.738, 9.938, 10.055, 11.016, 11.965, 12.551, 13.887, 14.695, 15.340, 15.434}, //RPM = 99.75 - { 1.031, 10.219, 10.945, 11.004, 11.566, 12.879, 13.758, 15.188, 16.559, 17.520, 17.613}, //RPM = 199.75 - { 1.125, 11.555, 11.801, 11.953, 13.090, 15.082, 16.160, 18.117, 19.289, 20.133, 20.145}, //RPM = 399.75 - { 1.277, 15.199, 15.609, 15.820, 17.438, 19.242, 20.766, 23.613, 26.156, 27.891, 27.891}, //RPM = 999.75 - { 1.523, 19.523, 19.863, 20.027, 21.516, 24.621, 26.215, 30.281, 34.359, 34.945, 34.945}, //RPM = 1599.75 - { 1.664, 21.434, 21.949, 22.078, 23.414, 27.832, 30.000, 34.793, 38.051, 38.039, 38.063}, //RPM = 1999.75 - } -}; - - -struct fuelMapIndexes fuelMapI = { //probar este mapa - { 100, 200, 400, 1000, 1600, 2000}, //N_RPM = 17 - { 0.031, 0.406, 1.656, 2.406, 8.313, 20.813, 29.094, 45.688, 60.000, 70.594, 72.000}, - { -14.2, 5.25, 29.9, 50, 75.3} -}; -#endif - - - - diff --git a/Core/Src/injection.c b/Core/Src/injection.c index 5d42767..16f85da 100644 --- a/Core/Src/injection.c +++ b/Core/Src/injection.c @@ -98,7 +98,7 @@ void INJ_UPDATE_ALPHA(){ //FM_GET_PHIAD(MT_RPM, ME, forceTemp); //correction_eoi_accel = TM_GET_ACCEL_CORRECTION(last_MT_RPM, MT_RPM, TEETH_RPM); - FM_GET_PHIAD(MT_RPM, ME, forceTemp); + FM_GET_PHIAD(MT_RPM, ME, Temp); float target = INJ_UPDATE_TARGET_EOI() + correction_eoi; @@ -110,8 +110,8 @@ void INJ_UPDATE_ALPHA(){ current_Alpha += TIMETODEG(INJ_CLOSING_MARGIN, TEETH_RPM); } current_Alpha = current_Alpha < 0.1 ? 0 : current_Alpha; - boostmult = BoostMultiplier(inj_mode, RPM, ME); - current_Alpha *= boostmult; + //boostmult = BoostMultiplier(inj_mode, RPM, ME); + //current_Alpha *= boostmult; } diff --git a/Core/Src/main.c b/Core/Src/main.c index 70f6bda..b0789e7 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -188,7 +188,11 @@ uint32_t lastPID_t = 0; //extern uint8_t forceDC2; void OnEnd(){ safetySHUTOFF = 1; + +#if defined(T06301) startup = 2; + +#endif startedEngine = 0; //forceDC2 = 2; @@ -268,7 +272,7 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { if (htim->Instance == TIM17) KL_Session_OnTim17Elapsed(); -} +}// /* USER CODE END 0 */ /** @@ -1132,7 +1136,7 @@ static void MX_TIM6_Init(void) htim6.Instance = TIM6; htim6.Init.Prescaler = 160-1; htim6.Init.CounterMode = TIM_COUNTERMODE_UP; - htim6.Init.Period = 11099; + htim6.Init.Period = 9999; htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; if (HAL_TIM_Base_Init(&htim6) != HAL_OK) { diff --git a/Core/Src/stm32g4xx_it.c b/Core/Src/stm32g4xx_it.c index 7b17bc9..62058e5 100644 --- a/Core/Src/stm32g4xx_it.c +++ b/Core/Src/stm32g4xx_it.c @@ -401,6 +401,7 @@ void TIM6_DAC_IRQHandler(void) /* USER CODE BEGIN TIM6_DAC_IRQn 1 */ //FBKW_PIDInterrupt(); FBKW_service(); + Timer1_FM_ISR(); /* USER CODE END TIM6_DAC_IRQn 1 */ } diff --git a/Core/Src/toothed_wheel.c b/Core/Src/toothed_wheel.c index 9914f3e..942512e 100644 --- a/Core/Src/toothed_wheel.c +++ b/Core/Src/toothed_wheel.c @@ -73,7 +73,7 @@ void TW_EVAL_MAX_TEETH(){ TW_RPM_SENSOR_STATE = (TW_current_max_teeth == TW_PERCYL_TEETH) ? 1 : 0 ; } uint32_t sensorfail; - +extern uint8_t startup_finished; volatile uint8_t isMT = 0; uint8_t SYNCOUT_TEETH = 0; volatile float accel_rpm = 0; @@ -137,7 +137,7 @@ void TW_TEETH_CAPTURE(){ TW_PREPARE_BOI_ACCELCORRECTION(); - if(startup && RPM > 200){ + if(!startup_finished && RPM > 200){ can_port_send_msg_def(&MSG_ID_EMPF3); // TX from template (+ symbols if any) Timeout_ResetByIndex(18, TIM16->CNT); // re-arm for another 40ms } @@ -271,7 +271,6 @@ void TW_CKP_CAPTURE(){ //meter timeout cuando !count_ckp (significa que solo recibe un pulso); CKP_PULSE_AVAILABLE = 1; Timeout_ResetByIndex(13, TIM16->CNT); // Reset CKP timeout - FBKW_CKP_ISR(); } } float fb_1; @@ -357,6 +356,7 @@ void TW_CALC_FBKW_FEEDBACK(){ difftop = fb_2 - FBKW_FEEDBACK_MAX; valid = 1; } + }else{ //feedbackfirst = RPM * 6.0 * diffCKP / refClock; if(feedbackMT){ @@ -376,7 +376,10 @@ void TW_CALC_FBKW_FEEDBACK(){ diffbot = FBKW_FEEDBACK_MIN - fb_1; difftop = fb_1 - FBKW_FEEDBACK_MAX; } + FBKW_CKP_ISR(); + } + } if(!valid && count_CKP > 1){ //si no encontro ningun otro nuevo valor dentro de rango, checkear boundaries diff --git a/hpsg5-controller_v2-stm32g4.ioc b/hpsg5-controller_v2-stm32g4.ioc index 5cdc0cc..13a3956 100644 --- a/hpsg5-controller_v2-stm32g4.ioc +++ b/hpsg5-controller_v2-stm32g4.ioc @@ -373,7 +373,7 @@ TIM4.PeriodNoDither=29851 TIM4.Prescaler=80-1 TIM4.PulseNoDither_2=5000 TIM6.IPParameters=Prescaler,PeriodNoDither -TIM6.PeriodNoDither=11100-1 +TIM6.PeriodNoDither=10000-1 TIM6.Prescaler=160-1 TIM7.IPParameters=Prescaler,PeriodNoDither,TIM_MasterOutputTrigger TIM7.PeriodNoDither=9