diff --git a/Core/CAN_Libs/can_db.c b/Core/CAN_Libs/can_db.c index c9a2162..32df8fd 100644 --- a/Core/CAN_Libs/can_db.c +++ b/Core/CAN_Libs/can_db.c @@ -60,6 +60,7 @@ extern float ME, MEPI, B_FB_KW, dFi, B_PHIAD; extern int16_t B_FB_NW; extern int16_t B_CKP_OFFSET; extern int8_t s_dfi_code; +extern int16_t rw3e, rw3c, rw164; extern uint8_t cilCount, safetySHUTOFF; extern uint8_t inj_mode, request_syncout_activation, commitCKP_offset; @@ -153,6 +154,10 @@ const CanAddressEntry CAN_ANSWERS[] = { { 0x7800, &T_ein, CAN_SYM_UX, &SCALE_TEIN , CAN_STORE_FLOAT }, { 0x4201, &PSG_Voltage, CAN_SYM_UX, &SCALE_VOLTAGE, CAN_STORE_FLOAT }, + { 0x3E00, &rw3e, CAN_SYM_SX, NULL, CAN_STORE_S16 }, + { 0x3C00, &rw3c, CAN_SYM_SX, NULL, CAN_STORE_S16 }, + + { 0x6401, &rw164, CAN_SYM_SX, NULL, CAN_STORE_S16 }, { 0x4000, &MT_RPM, CAN_SYM_UX, &SCALE_RPM_OUT, CAN_STORE_FLOAT }, { 0x3801, &MT_offset_RPM, CAN_SYM_UX, &SCALE_RPM_OUT, CAN_STORE_FLOAT }, @@ -307,7 +312,7 @@ static CanSymbolDef SYM_ID_SEND1[] = { #elif defined(T06215) { "MESOLL", 0, 12, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &ME, &SCALE_ME, CAN_STORE_FLOAT}, - { "B_PHIAD", 12, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_PHIAD, &SCALE_PHIAD, CAN_STORE_FLOAT}, + { "B_PHIAD", 16, 16, CAN_ENDIAN_INTEL, CAN_SYM_SX, 0,0, &B_PHIAD, &SCALE_PHIAD, CAN_STORE_FLOAT}, { "FB_KW", 32, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_FB_KW, &SCALE_FBKW, CAN_STORE_FLOAT}, { "FB_NW", 48, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_FB_NW, &SCALE_FBKW, CAN_STORE_FLOAT}, #elif defined(T15021) || defined(T31804) diff --git a/Core/Phi/phi.c b/Core/Phi/phi.c index d9db0fe..76b576a 100644 --- a/Core/Phi/phi.c +++ b/Core/Phi/phi.c @@ -349,6 +349,13 @@ reset_path: * 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 */ + +/* Forward declaration — compute_angle_accumulator_3d (defined below) embeds + * the Alternate_phiad_calc gate that conditionally calls this producer. + * The producer body sits further down in the file alongside its gated + * wrapper, so the forward decl lets the gate compile against the prototype. */ +static void compute_accel_comp_offset(runtime_state_t *rt, const calibration_t *cal); + static void compute_accel_comp_gain(runtime_state_t *rt, const calibration_t *cal) { submap_scratch_t scratch_rpm; @@ -432,8 +439,29 @@ static void compute_angle_accumulator_3d(runtime_state_t *rt, const calibration_ /* 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. */ + /* 0x728b–0x72af: Alternate_phiad_calc embedded gate (FUN_5df3 @ + * 0x5e50–0x5e62 in T06215, analog of T06031's FUN_5dd0 tail). + * + * Below the hysteresis threshold (gate_0220 == 0) the live ROM both + * recomputes the accel_comp_gain and force-flags REC.1 so the next + * Try_calc_accel_offset visit will produce a value despite low RPM. + * Stage 3 of phi_service has already run compute_accel_comp_gain + * unconditionally this tick, so we only need to mirror the REC.1 flag + * here. + * + * The compute_accel_comp_offset call is gated on REC.2 (set by + * compute_accel_comp_offset_gated when phi_per_cylinder_event has + * fired this cylinder cycle) AND REC.0 clear (consumer has drained + * any prior value). This is the PTS-event fallback path — at high + * RPM the per-cylinder hook will have already set REC.0, so this + * gate skips and the producer fires from the per-cylinder side + * exclusively. */ + if (rt->gate_0220 == 0u) { + rt->rec = (uint8_t)(rt->rec | 0x02u); + } + if (((rt->rec & 0x04u) != 0u) && ((rt->rec & 0x01u) == 0u)) { + compute_accel_comp_offset(rt, cal); + } } /* ══════════════════════════════════════════════════════════════════════ @@ -749,114 +777,41 @@ static void compute_accel_comp_offset(runtime_state_t *rt, const calibration_t * (void)rw1c; /* rw1c is discarded after MUL; preserved for block-mapping */ } -/* ====================================================================== - * tooth_isr / reset_tooth_state — port of FUN_51e1 @ 0x51e1 (producer) - * and FUN_5210 @ 0x5210 (DIVU helper) from the T06215 ROM. +/* + * 1:1 translation of FUN_5f33 @ 0x5f33–0x5f4d (T06215 ROM analog of + * T06031's FUN_736e). The "Try_calc_accel_offset" wrapper that conditionally + * calls compute_accel_comp_offset based on REC.0 / REC.1 / gate_0220. * - * The producer fires once per cylinder when R88 == cal_byte_56 (= 13). - * It computes rpm_baseline = 0x0F000000 / period_ticks where period is - * the elapsed EPA timer ticks between two consecutive phase-13 hits. - * ====================================================================== */ - -/* 0x0F000000 = 251,658,240 — the fixed dividend (cal+0xA2:cal+0xA4). */ -#define RPM_DIVIDEND_HI ((uint16_t)0x0F00u) -#define RPM_DIVIDEND_LO ((uint16_t)0x0000u) - -/* Period saturation (cal+0xA6:cal+0xA8) = 0x001B7740 = 1,800,000 ticks. */ -#define PERIOD_CLAMP_LO ((uint16_t)0x7740u) -#define PERIOD_CLAMP_HI ((uint16_t)0x001Bu) - -/* 0x5210–0x529c: period_to_rpm_div helper (FUN_5210). */ -static int16_t period_to_rpm_div(uint16_t period_lo, uint16_t period_hi) + * Triggered from the live-ROM Tooth_scheduler at 0x7abe, on the per-cylinder + * tooth event where R88 == cal_byte_56 — i.e. once per cylinder cycle, + * immediately after the rpm_baseline producer (Calculate_mid_rpm @ 0x51e1) + * updates *(0x0138). Wired into the public API as phi_per_cylinder_event. + * + * 0x5f37: ORB REC, #0x4 ; REC.2 := 1 (wrapper visited flag) + * 0x5f3a: JBS REC, 0x0, exit ; REC.0 set → fresh value pending, skip + * 0x5f3d: JBS REC, 0x1, compute ; REC.1 set → forced compute path + * 0x5f40: CMPB ZRlo, *(0x0220) ; gate_0220 (is_rpm_above_1000_hyst.) + * 0x5f45: JE exit ; gate_0220 == 0 → skip + * 0x5f47: SCALL FUN_5ef2 ; compute_accel_comp_offset + * 0x5f49: epilogue + */ +static void compute_accel_comp_offset_gated(runtime_state_t *rt, + const calibration_t *cal) { - /* 0x5255–0x526a: period saturation. */ - if (period_hi > PERIOD_CLAMP_HI) { - period_lo = PERIOD_CLAMP_LO; - period_hi = PERIOD_CLAMP_HI; - } else if (period_hi == PERIOD_CLAMP_HI) { - if (period_lo > PERIOD_CLAMP_LO) { - period_lo = PERIOD_CLAMP_LO; - period_hi = PERIOD_CLAMP_HI; - } - } + /* 0x5f37: REC |= 0x04 — set unconditionally, even on skip paths. */ + rt->rec = (uint8_t)(rt->rec | 0x04u); - /* 0x526f–0x5274: load dividend RL1C = 0x0F000000. */ - uint16_t rw1c = RPM_DIVIDEND_LO; - uint16_t rw1e = RPM_DIVIDEND_HI; - - /* 0x5279–0x527c: if high word == 0, simple 32÷16 DIVU. */ - if (period_hi == 0u) { - /* 0x527e: DIVU RL1C, RW80. */ - if (period_lo == 0u) { - return 0; - } - uint32_t dividend = ((uint32_t)rw1e << 16) | (uint32_t)rw1c; - rw1c = (uint16_t)(dividend / (uint32_t)period_lo); - } else { - /* 0x5283–0x5295: long-period scaling loop. */ - uint8_t shift_count = 0u; - uint32_t divisor = ((uint32_t)period_hi << 16) | (uint32_t)period_lo; - while ((divisor >> 16) != 0u) { - divisor >>= 1; - shift_count++; - } - uint32_t dividend = ((uint32_t)rw1e << 16) | (uint32_t)rw1c; - uint16_t scaled_divisor = (uint16_t)divisor; - if (scaled_divisor == 0u) { - return 0; - } - rw1c = (uint16_t)(dividend / (uint32_t)scaled_divisor); - /* 0x5292: SHR RW1C, R20 — rescale quotient. */ - rw1c >>= shift_count; - } - - return (int16_t)rw1c; -} - -void phi_tooth_isr(phi_state_t *state, - const phi_cal_t *cal, - uint8_t current_tooth, - uint16_t current_capture) -{ - runtime_state_t *rt = &state->rt; - - /* 0x7aa4–0x7aa9: gate on phase comparand. */ - if (current_tooth != cal->cal_byte_56) { + /* 0x5f3a: REC.0 set → consumer (FUN_7453) hasn't drained the previous + * RW3C yet; skip recomputation to avoid losing the pending value. */ + if ((rt->rec & 0x01u) != 0u) { return; } - /* First call after reset: seed snapshot, skip divide. */ - if (!rt->mt_rpm_seeded) { - rt->mt_rpm_capture_prev = current_capture; - rt->mt_rpm_index_prev = 0u; - rt->rpm_baseline = 0; - rt->mt_rpm_seeded = 1u; - return; + /* 0x5f3d–0x5f47: REC.1 forces the compute path; otherwise gate on + * the live-byte hysteresis at *(0x0220). */ + if (((rt->rec & 0x02u) != 0u) || (rt->gate_0220 != 0u)) { + compute_accel_comp_offset(rt, cal); } - - /* 0x5247–0x524b: SUB RW80, RW64, [RW1C]+ / SUBC RW82, [RW1C]. */ - uint16_t period_lo = (uint16_t)(current_capture - rt->mt_rpm_capture_prev); - uint16_t borrow = (current_capture < rt->mt_rpm_capture_prev) ? 1u : 0u; - uint16_t period_hi = (uint16_t)(0u - rt->mt_rpm_index_prev - borrow); - - /* 0x524e–0x5251: persist new snapshot. */ - rt->mt_rpm_capture_prev = current_capture; - rt->mt_rpm_index_prev = 0u; - - /* 0x5255–0x5298: helper → quotient. */ - int16_t result = period_to_rpm_div(period_lo, period_hi); - - /* 0x51ec: ST RW1C, 0x138. */ - rt->rpm_baseline = result; -} - -void phi_reset_tooth_state(phi_state_t *state) -{ - runtime_state_t *rt = &state->rt; - rt->rpm_baseline = 0; - rt->mt_rpm_capture_prev = 0u; - rt->mt_rpm_index_prev = 0u; - rt->mt_rpm_seeded = 0u; } /* @@ -1162,10 +1117,6 @@ void phi_init(phi_state_t *state, const phi_cal_t *cal, * already zeroed rt->rw9c / rt->rw9e (mirrors FUN_6ba3 @ 0x6bbf–0x6bc1). */ state->rt.temp_phi_comp_r94 = 100u; - /* Tooth-ISR pipeline: memset already zeroed mt_rpm_capture_prev, - * mt_rpm_index_prev, rpm_baseline, and mt_rpm_seeded — matching - * reset_toothed_wheel @ 0x5301 boot behaviour. */ - /* 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 @@ -1192,8 +1143,13 @@ void phi_service(phi_state_t *state, const phi_cal_t *cal, phi_outputs_t *out) rt->inj_qty_demand = g->get_inj_qty_demand(); rt->angle_dec_cmd = g->get_angle_dec_cmd(); rt->temperature = g->get_temperature(); - /* rpm_baseline is now produced internally by tooth_isr (FUN_51e1); - * no longer pulled from a getter. */ + /* rt->rpm_baseline is intentionally NOT pulled here — its cadence is + * tooth-13-event-driven, not main-loop-driven. The pull happens at + * the two consumer-side entry points instead: + * - phi_per_cylinder_event (the per-cylinder hook) + * - just before compute_angle_accumulator_3d in Stage 3b (the + * PTS-event fallback path's embedded Alternate_phiad_calc gate). + * See the comment at each pull site. */ rt->rwc2 = g->get_rwc2(); rt->reset_gate_0226 = g->get_reset_gate_0226(); rt->dphi = g->get_dphi(); @@ -1234,9 +1190,18 @@ void phi_service(phi_state_t *state, const phi_cal_t *cal, phi_outputs_t *out) * 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. */ + * angle-kick stage (3d) reuses without re-evaluating. + * + * The Alternate_phiad_calc embedded gate at the tail of this function + * may fire compute_accel_comp_offset (the "PTS-event fallback path"). + * Pull rpm_baseline FRESH right before this gate so the producer reads + * the latest tooth-13 snapshot rather than whatever stale value Stage + * 1 left behind — phi_service's main-loop cadence is not synchronised + * with the per-cylinder tooth-13 event, so a Stage-1 pull may capture + * the previous cycle's baseline and invert RW3C polarity. Mirrors the + * pull at the top of phi_per_cylinder_event so both producer entry + * points see the same snapshot ordering. */ + rt->rpm_baseline = g->get_rpm_baseline(); compute_angle_accumulator_3d(rt, cal); /* ── Stage 3c: compute_temp_comp_factor (orphan calc_temp_comp_factor @ 0x6AE7) ─ @@ -1265,14 +1230,22 @@ void phi_service(phi_state_t *state, const phi_cal_t *cal, phi_outputs_t *out) 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 4: compute_accel_comp_offset is no longer called here ── + * The producer is now driven by the live-ROM cadence: + * + * 1. Per-cylinder path — host calls phi_per_cylinder_event() at the + * cal_byte_56 tooth (mirrors Tooth_scheduler @ 0x7abe). This + * sets REC.2 and may run compute_accel_comp_offset depending on + * gate_0220 / REC.1 state. + * + * 2. PTS-event fallback path — the Alternate_phiad_calc embedded + * gate at the tail of compute_angle_accumulator_3d (Stage 3b) + * runs compute_accel_comp_offset when REC.2 is set AND REC.0 + * is clear. + * + * Calling the bare producer here would overwrite RW3C every tick + * regardless of cadence, which is the pre-2026-05-06 divergence the + * live-ECU comparison surfaced. */ /* ── Stage 5: compute_gate_0220 (orphan @ 0x77ff) ─────────────── * Updates rt->gate_0220 from current rpm with hysteresis bands at @@ -1323,3 +1296,47 @@ void phi_service(phi_state_t *state, const phi_cal_t *cal, phi_outputs_t *out) size_t phi_state_size(void) { return sizeof(phi_state_t); } size_t phi_cal_size(void) { return sizeof(phi_cal_t); } + +/* + * phi_per_cylinder_event — public per-cylinder hook. + * + * Models the live-ROM `Tooth_scheduler` dispatch at 0x7abe: + * + * if (R88 == cal_byte_56) { // 0x7aa9 JE LAB_7abb + * Calculate_mid_rpm(); // 0x7abb LCALL FUN_51e1 + * Try_calc_accel_offset(); // 0x7abe LCALL FUN_5f33 + * } + * + * The host is responsible for the `Calculate_mid_rpm` analog: write the + * fresh rpm_baseline (RW138) into `state->rt.rpm_baseline` (or via the + * getter) BEFORE calling this hook. This routine then runs the + * Try_calc_accel_offset wrapper (compute_accel_comp_offset_gated), which + * sets REC.2 and conditionally calls compute_accel_comp_offset. + * + * Cadence (per the live-ECU side-by-side traces): + * - High RPM (gate_0220 == 1): hook fires the producer; REC.0 is set; + * phi_service's Stage 3b fallback gate observes REC.0 and skips. + * - Low RPM (gate_0220 == 0): hook only sets REC.2; phi_service's + * Stage 3b runs the producer because REC.1 was set there and REC.0 + * is still clear. + * - In both cases compute_target_injection_angle (Stage 7) clears the + * entire REC byte after consuming RW3C, so REC reads back as 0 + * between events — matching the live observation that *(0x00ec) + * stays at zero on the bus probe. + * + * Reentrant per phi_state_t. Safe to invoke from a tooth-edge ISR + * provided the host serialises against phi_service. + * + * Pulls rpm_baseline from the getter at hook entry — the host computes + * the fresh tooth-13 snapshot just before invoking this hook, but + * phi_service's Stage 1 pull captures rpm_baseline at main-loop cadence + * (independent of tooth-13 events), leaving rt->rpm_baseline stale at + * this moment. The fresh pull here guarantees the producer sees the + * just-computed snapshot. The mirror pull just before Stage 3b in + * phi_service does the same for the PTS-event fallback path. + */ +void phi_per_cylinder_event(phi_state_t *state, const phi_cal_t *cal) +{ + state->rt.rpm_baseline = state->getters->get_rpm_baseline(); + compute_accel_comp_offset_gated(&state->rt, cal); +} diff --git a/Core/Phi/phi.h b/Core/Phi/phi.h index 752bdf2..513710c 100644 --- a/Core/Phi/phi.h +++ b/Core/Phi/phi.h @@ -40,11 +40,10 @@ * 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. + * FUN_5D58 angle_kick + `RW52 += RW3E` fold-in @ 0x7A3E) is wired into + * this compact phi_service. RW9E (the IIR state high word consumed by + * compute_temp_comp_factor) is produced internally by phi_tick_1khz — + * the host must drive it once per 1 ms from its own millisecond timer. * See variants/T06211/docs/open-questions.md §9. */ #ifndef PHI_T06215_H @@ -102,14 +101,7 @@ typedef struct { int16_t dphi; /* *(0x014c) — angle delta. */ /* ---- Inputs to FUN_732d (compute_accel_comp_offset) ---- */ - int16_t rpm_baseline; /* *(0x0138) — RPM baseline produced internally by tooth_isr - * (FUN_51e1 @ 0x51e1). No longer an external input. */ - - /* ---- Tooth-ISR pipeline state (FUN_51e1 @ 0x51e1 + FUN_5210 @ 0x5210) ---- */ - uint16_t mt_rpm_capture_prev; /* *(0x02A8) — RW64 from previous phase-13 hit */ - uint16_t mt_rpm_index_prev; /* *(0x02AA) — RW7E (capture index) at previous hit */ - uint8_t mt_rpm_seeded; /* 0 = first phase-13 after reset; skip divide and seed only */ - + 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. */ @@ -124,12 +116,12 @@ typedef struct { * 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 + * state pair updated by phi_tick_1khz (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. */ + * compute_temp_comp_factor at 0x5DC0. */ 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 @@ -157,7 +149,11 @@ typedef struct { 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_56; /* CAL_byte[0x56] — tooth phase comparand for rpm_baseline producer (= 0x0D = 13). */ + uint8_t cal_byte_56; /* CAL_byte[0x56] — tooth phase comparand for the ROM's rpm_baseline + * producer (T06211 producer @ 0x5153 fires when current_tooth == this + * byte; = 0x0D = 13). The C port treats rpm_baseline as an external + * input — this byte is published so the host knows which tooth phase + * to gate its capture on. */ 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. */ @@ -166,9 +162,9 @@ typedef struct { 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 + int16_t cal_82; /* CAL[0x82] — IIR input-gain `b` (Q16 unsigned) used by phi_tick_1khz * 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 + int16_t cal_84; /* CAL[0x84] — IIR pole `a` (Q16 unsigned) used by phi_tick_1khz * 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). */ @@ -224,6 +220,7 @@ typedef struct { 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) */ @@ -258,18 +255,31 @@ void phi_service(phi_state_t *state, * runtime_state_t. */ void phi_tick_1khz(phi_state_t *state, const phi_cal_t *cal); -/** Per-accepted-tooth ISR entry point. Updates rt->rpm_baseline when - * current_tooth == cal->cal_byte_56 (=13). No-op for other phases. - * The caller invokes this once per accepted tooth event at HSI.0 rate, - * separate from the phi_service scheduler cadence. */ -void phi_tooth_isr(phi_state_t *state, - const phi_cal_t *cal, - uint8_t current_tooth, - uint16_t current_capture); - -/** Clear the tooth-ISR pipeline (mirrors reset_toothed_wheel @ 0x5301). - * Next phi_tooth_isr call will seed without dividing. */ -void phi_reset_tooth_state(phi_state_t *state); +/** Per-cylinder hook. Host invokes once per cylinder cycle, on the tooth + * edge where `current_tooth == cal->cal_byte_56`, AFTER the host has + * written the fresh rpm_baseline (RW138) into the runtime state. + * + * Mirrors the live-ROM Tooth_scheduler dispatch at 0x7abe (T06215): + * + * if (R88 == cal_byte_56) { + * Calculate_mid_rpm(); // host responsibility (writes rpm_baseline) + * Try_calc_accel_offset(); // this function + * } + * + * Internally calls the FUN_5f33 analog (compute_accel_comp_offset_gated) + * which sets REC.2 unconditionally and then conditionally fires + * compute_accel_comp_offset based on REC.0 / REC.1 / gate_0220: + * - REC.0 set → skip (fresh value already pending) + * - REC.1 set OR gate_0220 != 0 → run producer (writes RW3C, sets REC.0) + * - otherwise → REC.2 only, no compute + * + * This is the cadence-matched entry point. Hosts that previously relied + * on phi_service alone to drive RW3C should switch to: + * per cylinder edge → phi_per_cylinder_event(...) + * per main control tick → phi_service(...) + * + * Reentrant per phi_state_t. */ +void phi_per_cylinder_event(phi_state_t *state, const phi_cal_t *cal); size_t phi_state_size(void); size_t phi_cal_size(void); diff --git a/Core/Phi/phi_cal_tables.c b/Core/Phi/phi_cal_tables.c index 899ebc1..84209e6 100644 --- a/Core/Phi/phi_cal_tables.c +++ b/Core/Phi/phi_cal_tables.c @@ -159,7 +159,7 @@ calibration_t phi_t06215_cal = { .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_56 = 0x0Du, /* CAL+0x56 @ 0x9C2E -- tooth phase comparand for rpm_baseline producer (FUN_51e1 at Tooth_scheduler? 0x7aa4) */ + .cal_byte_56 = 0x0Du, /* CAL+0x56 @ 0x9C2E -- tooth phase comparand for the ROM's rpm_baseline producer (FUN_51e1 @ 0x51e1; T06211 analog @ 0x5153). The C port treats rpm_baseline as an external input — this byte is published so the host knows which tooth phase to gate its capture on. */ .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 */ diff --git a/Core/Src/fuel_map.c b/Core/Src/fuel_map.c index e1896c5..e468570 100644 --- a/Core/Src/fuel_map.c +++ b/Core/Src/fuel_map.c @@ -23,6 +23,7 @@ #include "phi.h" #include #include +#include "ee_manager.h" #define CF_TMS 8.388f #define CF_ME 32.0f @@ -34,6 +35,8 @@ /* 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_rpm_offset = 0.0f; + static float s_me = 0.0f; static float s_temp = 0.0f; @@ -44,6 +47,13 @@ static phi_input_getters_t s_phi_getters; static phi_outputs_t s_phi_out; static float *s_phiad_out = NULL; +float s_c2; +float s_103; +float s_108; + +extern float MT_offset_RPM; +extern float B_PHIAD; + /* ──── Getters (nullary; T06215 vtable shape) ────────────────────────── */ static uint16_t get_rpm(void) { return (uint16_t)(s_rpm * CF_TMS); } @@ -55,18 +65,18 @@ static int16_t get_temperature(void) { return (int16_t) (s_temp * CF_T_M + * extern float B_PHIAD; * return (int16_t)(B_PHIAD * CF_KW); */ -static int16_t get_angle_dec_cmd(void) { return 0; } +static int16_t get_angle_dec_cmd(void) { return (uint16_t)(B_PHIAD * CF_KW); } /* 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 int16_t get_rpm_baseline(void) { return (int16_t)(MT_offset_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; } +static int16_t get_dphi(void) { return (int16_t)(s_dfi_code); } +static uint8_t get_scratch_0103(void) { return 0x78; } /* matches sim_t06215_sweep.c */ +static uint8_t get_scratch_0108(void) { return 0x9A; } /* ──── Public API ─────────────────────────────────────────────────────── */ @@ -77,9 +87,9 @@ void init_FuelMap(float *PHIAD) { 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_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_reset_gate_0226 = get_reset_gate_0226; //this is is_accel_comp_disabled 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; @@ -98,22 +108,32 @@ void Timer1_FM_ISR(){ phi_tick_1khz(&s_phi_state, &s_phi_cal); } -void TW_FM_ISR(uint8_t currentTooth, uint32_t ic){ - phi_tooth_isr(&s_phi_state, &s_phi_cal, currentTooth, (uint16_t)(ic >> 4)); -} +extern float forceTemp, MT_RPM; +int16_t rw3e = 0; +int16_t rw3c = 0; +int16_t rw164 = 0; -extern float forceTemp; float FM_GET_PHIAD(float RPM, float ME, float Temp) { - s_rpm = RPM; + s_rpm = MT_RPM; + //s_rpm_offset = MT_offset_RPM; s_me = ME; - s_temp = Temp; + s_temp = forceTemp; phi_service(&s_phi_state, &s_phi_cal, &s_phi_out); float Z = (float)s_phi_out.angle_accumulator * ANGLE_DEG_PER_RAW; + rw3e = s_phi_out.angle_kick_2d; + rw164 = s_phi_out.accel_comp_gain; + rw3c = s_phi_out.accel_comp_offset; + Z = s_me ? Z : 0; // el terrano se apagaba if (s_phiad_out) *s_phiad_out = Z; return Z; } +void TW_midrpm_isr(){ + phi_per_cylinder_event(&s_phi_state, &s_phi_cal); + +} + diff --git a/Core/Src/injection.c b/Core/Src/injection.c index 16f85da..f233438 100644 --- a/Core/Src/injection.c +++ b/Core/Src/injection.c @@ -89,6 +89,9 @@ extern float last_accel; uint8_t awaitingInj = 0; float INJ_GET_NOMINAL_EOI(); +void INJ_UPDATE_PHIAD(){ + FM_GET_PHIAD(MT_RPM, ME, Temp); +} void INJ_UPDATE_ALPHA(){ /*float rp_sum = MT_RPM + last_MT_RPM; float dt = (PHI1 + dFi) / 90 * (1.0f / CYLINDERS) * (120.0f / rp_sum); @@ -98,7 +101,6 @@ 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, Temp); float target = INJ_UPDATE_TARGET_EOI() + correction_eoi; @@ -138,7 +140,7 @@ void INJ_PREPARE_ONCE(){ TIM1->PSC = 20-1; } INJ_PREPARE_BIP(0); - INJ_UPDATE_ALPHA(); + //INJ_UPDATE_ALPHA(); INJ_UPDATE_TYPE(); } @@ -148,7 +150,7 @@ void INJ_PREPARE_BIP(uint8_t teeth){ if(teeth < triggerTeeth || !triggerTeeth) { INJ_UPDATE_BOI_TRIGGER(); - INJ_UPDATE_ALPHA(); + //INJ_UPDATE_ALPHA(); } //correction_boi_accel_2 = TM_UPDATE_ACCEL_CORRECTION_BIP(last_MT_RPM, MT_RPM, TEETH_RPM); //correction_beta = s_boi_corr_deg + correction_boi_accel_2; @@ -470,7 +472,8 @@ void SEND1_Handler( #if defined(T06301) startupiscar = 1; #endif - if(!isInjecting && !hasInjectionEnded){ + INJ_UPDATE_PHIAD(); + if(!isInjecting && !hasInjectionEnded){// INJ_UPDATE_ALPHA(); } } diff --git a/Core/Src/toothed_wheel.c b/Core/Src/toothed_wheel.c index 5eb739a..9d64d4a 100644 --- a/Core/Src/toothed_wheel.c +++ b/Core/Src/toothed_wheel.c @@ -195,11 +195,14 @@ void TW_TEETH_CAPTURE(){ } INJ_EVAL_END(); if(currentTooth == 13){ + RPM_offset_Difference = IC_RPM_Val2 - edgeBuf[currentTooth].ic; MT_offset_RPM = fclamp(60.0 * (refClock/RPM_offset_Difference) / CYLINDERS, MIN_RPM, 4000); if(MT_RPM <15){ MT_offset_RPM = MT_RPM; } + TW_midrpm_isr(); + } UpdateEdgeBuffer(IC_RPM_Val2, TEETH_RPM, currentTooth, isInjecting); @@ -233,9 +236,6 @@ void TW_TEETH_CAPTURE(){ if(currentTooth == TW_PERCYL_TEETH - 1){ INJ_END(); } - TW_FM_ISR(currentTooth,IC_RPM_Val2 ); - - hasCapturedTeeth = 1;