Firmware/hpsg5-controller_v2-stm32g4
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

la compensacion de aceleracion va perfecto. Muy parecido al original. cambiado el send1 handler y añadido un call desde tooth 13 para calcular la comp accel en una zona determinada

Browse Source
This commit is contained in:
LucianoDev
2026-05-06 16:35:06 +02:00
parent 98a835026e
commit 7ff62ccabc
7 changed files with 227 additions and 172 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
Core/CAN_Libs/can_db.c
View File

@@ -60,6 +60,7 @@ extern float ME, MEPI, B_FB_KW, dFi, B_PHIAD;
extern int16_t B_FB_NW; extern int16_t B_FB_NW;
extern int16_t B_CKP_OFFSET; extern int16_t B_CKP_OFFSET;
extern int8_t s_dfi_code; extern int8_t s_dfi_code;
extern int16_t rw3e, rw3c, rw164;
extern uint8_t cilCount, safetySHUTOFF; extern uint8_t cilCount, safetySHUTOFF;
extern uint8_t inj_mode, request_syncout_activation, commitCKP_offset; 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 }, { 0x7800, &T_ein, CAN_SYM_UX, &SCALE_TEIN , CAN_STORE_FLOAT },
{ 0x4201, &PSG_Voltage, CAN_SYM_UX, &SCALE_VOLTAGE, 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 }, { 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 }, { 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) #elif defined(T06215)
{ "MESOLL", 0, 12, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &ME, &SCALE_ME, CAN_STORE_FLOAT}, { "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_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}, { "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) #elif defined(T15021) || defined(T31804)

257
Core/Phi/phi.c
View File

@@ -349,6 +349,13 @@ reset_path:
* 5. refine_submap_result over the 1-D table at CAL+0x72 * 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 * 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) static void compute_accel_comp_gain(runtime_state_t *rt, const calibration_t *cal)
{ {
submap_scratch_t scratch_rpm; 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. */ /* 0x7288–0x728a: LD RW52, RW1C — write angle_accumulator. */
rt->angle_accumulator = result; rt->angle_accumulator = result;
/* 0x728b–0x72af: conditional accel_comp_gain / accel_comp_offset /* 0x728b–0x72af: Alternate_phiad_calc embedded gate (FUN_5df3 @
* sub-calls — handled by Stages 3 and 4 of phi_service. */ * 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 */ (void)rw1c; /* rw1c is discarded after MUL; preserved for block-mapping */
} }
/* ====================================================================== /*
* tooth_isr / reset_tooth_state — port of FUN_51e1 @ 0x51e1 (producer) * 1:1 translation of FUN_5f33 @ 0x5f33–0x5f4d (T06215 ROM analog of
* and FUN_5210 @ 0x5210 (DIVU helper) from the T06215 ROM. * 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). * Triggered from the live-ROM Tooth_scheduler at 0x7abe, on the per-cylinder
* It computes rpm_baseline = 0x0F000000 / period_ticks where period is * tooth event where R88 == cal_byte_56 — i.e. once per cylinder cycle,
* the elapsed EPA timer ticks between two consecutive phase-13 hits. * immediately after the rpm_baseline producer (Calculate_mid_rpm @ 0x51e1)
* ====================================================================== */ * updates *(0x0138). Wired into the public API as phi_per_cylinder_event.
*
/* 0x0F000000 = 251,658,240 — the fixed dividend (cal+0xA2:cal+0xA4). */ * 0x5f37: ORB REC, #0x4 ; REC.2 := 1 (wrapper visited flag)
#define RPM_DIVIDEND_HI ((uint16_t)0x0F00u) * 0x5f3a: JBS REC, 0x0, exit ; REC.0 set → fresh value pending, skip
#define RPM_DIVIDEND_LO ((uint16_t)0x0000u) * 0x5f3d: JBS REC, 0x1, compute ; REC.1 set → forced compute path
* 0x5f40: CMPB ZRlo, *(0x0220) ; gate_0220 (is_rpm_above_1000_hyst.)
/* Period saturation (cal+0xA6:cal+0xA8) = 0x001B7740 = 1,800,000 ticks. */ * 0x5f45: JE exit ; gate_0220 == 0 → skip
#define PERIOD_CLAMP_LO ((uint16_t)0x7740u) * 0x5f47: SCALL FUN_5ef2 ; compute_accel_comp_offset
#define PERIOD_CLAMP_HI ((uint16_t)0x001Bu) * 0x5f49: epilogue
*/
/* 0x5210–0x529c: period_to_rpm_div helper (FUN_5210). */ static void compute_accel_comp_offset_gated(runtime_state_t *rt,
static int16_t period_to_rpm_div(uint16_t period_lo, uint16_t period_hi) const calibration_t *cal)
{ {
/* 0x5255–0x526a: period saturation. */ /* 0x5f37: REC |= 0x04 — set unconditionally, even on skip paths. */
if (period_hi > PERIOD_CLAMP_HI) { rt->rec = (uint8_t)(rt->rec | 0x04u);
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;
}
}
/* 0x526f–0x5274: load dividend RL1C = 0x0F000000. */ /* 0x5f3a: REC.0 set → consumer (FUN_7453) hasn't drained the previous
uint16_t rw1c = RPM_DIVIDEND_LO; * RW3C yet; skip recomputation to avoid losing the pending value. */
uint16_t rw1e = RPM_DIVIDEND_HI; if ((rt->rec & 0x01u) != 0u) {
/* 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) {
return; return;
} }
/* First call after reset: seed snapshot, skip divide. */ /* 0x5f3d–0x5f47: REC.1 forces the compute path; otherwise gate on
if (!rt->mt_rpm_seeded) { * the live-byte hysteresis at *(0x0220). */
rt->mt_rpm_capture_prev = current_capture; if (((rt->rec & 0x02u) != 0u) || (rt->gate_0220 != 0u)) {
rt->mt_rpm_index_prev = 0u; compute_accel_comp_offset(rt, cal);
rt->rpm_baseline = 0;
rt->mt_rpm_seeded = 1u;
return;
} }
/* 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). */ * already zeroed rt->rw9c / rt->rw9e (mirrors FUN_6ba3 @ 0x6bbf–0x6bc1). */
state->rt.temp_phi_comp_r94 = 100u; 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 /* 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 * ROM this is written by FUN_5a97 with an RPM-correlated condition
* via RW68 (whose actual writer lives in the unreadable ROM gap * 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->inj_qty_demand = g->get_inj_qty_demand();
rt->angle_dec_cmd = g->get_angle_dec_cmd(); rt->angle_dec_cmd = g->get_angle_dec_cmd();
rt->temperature = g->get_temperature(); rt->temperature = g->get_temperature();
/* rpm_baseline is now produced internally by tooth_isr (FUN_51e1); /* rt->rpm_baseline is intentionally NOT pulled here — its cadence is
* no longer pulled from a getter. */ * 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->rwc2 = g->get_rwc2();
rt->reset_gate_0226 = g->get_reset_gate_0226(); rt->reset_gate_0226 = g->get_reset_gate_0226();
rt->dphi = g->get_dphi(); 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 * FUN_7092 (3-D trilinear = combine_three_submaps_to_word) → writes
* rt->angle_accumulator (RW52) and rt->scratch_0158 debug mirror. * rt->angle_accumulator (RW52) and rt->scratch_0158 debug mirror.
* Also populates rt->scratch_rpm and rt->scratch_demand which the * Also populates rt->scratch_rpm and rt->scratch_demand which the
* angle-kick stage (3d) reuses without re-evaluating. The ROM's * angle-kick stage (3d) reuses without re-evaluating.
* conditional gain/offset sub-calls inside FUN_722e (FUN_72b0 and *
* FUN_732d at 0x7292 / 0x729d) are handled by Stage 3 and Stage 4. */ * 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); compute_angle_accumulator_3d(rt, cal);
/* ── Stage 3c: compute_temp_comp_factor (orphan calc_temp_comp_factor @ 0x6AE7) ─ /* ── 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; rt->angle_accumulator = 0;
} }
/* ── Stage 4: compute_accel_comp_offset (FUN_732d @ 0x732d) ───── /* ── Stage 4: compute_accel_comp_offset is no longer called here ──
* Δ-rpm × accel_comp_gain → high-word of (signed MUL << 4), * The producer is now driven by the live-ROM cadence:
* clamped against [cal_7a, cal_78]. Forced to 0 when *
* reset_gate_0226 == 0, then ORs REC with 0x01 so FUN_7453's reset * 1. Per-cylinder path — host calls phi_per_cylinder_event() at the
* branch folds RW3C into the accumulator next tick. We invoke the * cal_byte_56 tooth (mirrors Tooth_scheduler @ 0x7abe). This
* raw producer (not the gated wrapper FUN_736e) to match the spec — * sets REC.2 and may run compute_accel_comp_offset depending on
* the gate_0220 update in stage 5 is informational only. */ * gate_0220 / REC.1 state.
compute_accel_comp_offset(rt, cal); *
* 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) ─────────────── /* ── Stage 5: compute_gate_0220 (orphan @ 0x77ff) ───────────────
* Updates rt->gate_0220 from current rpm with hysteresis bands at * 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_state_size(void) { return sizeof(phi_state_t); }
size_t phi_cal_size(void) { return sizeof(phi_cal_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);
}

70
Core/Phi/phi.h
View File

@@ -40,11 +40,10 @@
* 7. compute_target_injection_angle (FUN_7453) -> RW48 / RW5A * 7. compute_target_injection_angle (FUN_7453) -> RW48 / RW5A
* *
* The orphan temperature-kick chain (compute_temp_comp_factor → orphan * The orphan temperature-kick chain (compute_temp_comp_factor → orphan
* FUN_5D58 angle_kick + `RW52 += RW3E` fold-in @ 0x7A3E) is ported in * FUN_5D58 angle_kick + `RW52 += RW3E` fold-in @ 0x7A3E) is wired into
* variants/T06215/src/ and wired into this compact phi_service. * this compact phi_service. RW9E (the IIR state high word consumed by
* RW9E (the IIR state high word consumed by compute_temp_comp_factor) * compute_temp_comp_factor) is produced internally by phi_tick_1khz —
* is produced internally by compute_temp_phi_comp — see * the host must drive it once per 1 ms from its own millisecond timer.
* compute_temp_phi_comp.h for the 1 kHz hook the host must drive.
* See variants/T06211/docs/open-questions.md §9. * See variants/T06211/docs/open-questions.md §9.
*/ */
#ifndef PHI_T06215_H #ifndef PHI_T06215_H
@@ -102,14 +101,7 @@ typedef struct {
int16_t dphi; /* *(0x014c) — angle delta. */ int16_t dphi; /* *(0x014c) — angle delta. */
/* ---- Inputs to FUN_732d (compute_accel_comp_offset) ---- */ /* ---- Inputs to FUN_732d (compute_accel_comp_offset) ---- */
int16_t rpm_baseline; /* *(0x0138) — RPM baseline produced internally by tooth_isr int16_t rpm_baseline; /* *(0x0138) — slow RPM baseline. */
* (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 accel_comp_gain; /* *(0x0164) — Δ-rpm × gain multiplier. */ int16_t accel_comp_gain; /* *(0x0164) — Δ-rpm × gain multiplier. */
uint8_t reset_gate_0226; /* *(0x0226) — accel_comp_offset enable gate. */ uint8_t reset_gate_0226; /* *(0x0226) — accel_comp_offset enable gate. */
uint8_t gate_0220; /* *(0x0220) — RPM-hysteresis 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. */ * 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 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 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). */ * @ 0x6b4e). Boot-zero (FUN_6ba3 @ 0x6bbf clears it). */
int16_t rw9e; /* RW9E @ 0x009E — high word of the {rw9e:rw9c} signed IIR-filter 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 * state pair, sole writer is phi_tick_1khz at the 10 Hz
* derived rate (port of FUN_6b2a @ 0x6b2a). Consumed by * 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. 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 * 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 * 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 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 cal_48; /* CAL[0x48] — value loaded into RW7A when FUN_62a2 fires (= 0x04B0). */
int16_t phi0; /* CAL[0x4c] — base/initial angle. */ 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). */ 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_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_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_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_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_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. */ * 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. * at the FUN_6b4e MULUs @ 0x6B4E / 0x6B57. Read via (uint16_t) cast.
* On this ROM cal_82 + (uint16_t)cal_84 == 0x10000 (unity DC). */ * 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_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 */ int16_t (*get_angle_dec_cmd)(void); /* RW42 */
uint16_t (*get_rpm)(void); /* RW40 */ uint16_t (*get_rpm)(void); /* RW40 */
int16_t (*get_temperature)(void); /* *(0x0146) */ int16_t (*get_temperature)(void); /* *(0x0146) */
int16_t (*get_rpm_baseline)(void); /* *(0x0138) */
uint16_t (*get_rwc2)(void); /* RWC2 */ uint16_t (*get_rwc2)(void); /* RWC2 */
uint8_t (*get_reset_gate_0226)(void); /* *(0x0226) */ uint8_t (*get_reset_gate_0226)(void); /* *(0x0226) */
int16_t (*get_dphi)(void); /* *(0x014c) */ int16_t (*get_dphi)(void); /* *(0x014c) */
@@ -258,18 +255,31 @@ void phi_service(phi_state_t *state,
* runtime_state_t. */ * runtime_state_t. */
void phi_tick_1khz(phi_state_t *state, const phi_cal_t *cal); void phi_tick_1khz(phi_state_t *state, const phi_cal_t *cal);
/** Per-accepted-tooth ISR entry point. Updates rt->rpm_baseline when /** Per-cylinder hook. Host invokes once per cylinder cycle, on the tooth
* current_tooth == cal->cal_byte_56 (=13). No-op for other phases. * edge where `current_tooth == cal->cal_byte_56`, AFTER the host has
* The caller invokes this once per accepted tooth event at HSI.0 rate, * written the fresh rpm_baseline (RW138) into the runtime state.
* separate from the phi_service scheduler cadence. */ *
void phi_tooth_isr(phi_state_t *state, * Mirrors the live-ROM Tooth_scheduler dispatch at 0x7abe (T06215):
const phi_cal_t *cal, *
uint8_t current_tooth, * if (R88 == cal_byte_56) {
uint16_t current_capture); * Calculate_mid_rpm(); // host responsibility (writes rpm_baseline)
* Try_calc_accel_offset(); // this function
/** 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); * 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_state_size(void);
size_t phi_cal_size(void); size_t phi_cal_size(void);

2
Core/Phi/phi_cal_tables.c
View File

@@ -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) */ .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) */ .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 */ .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_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_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_74 = (int16_t)0x20C5, /* CAL+0x74 @ 0x9C4C -- gate_0220 lower RPM threshold */

46
Core/Src/fuel_map.c
View File

@@ -23,6 +23,7 @@
#include "phi.h" #include "phi.h"
#include <stdint.h> #include <stdint.h>
#include <stddef.h> #include <stddef.h>
#include "ee_manager.h"
#define CF_TMS 8.388f #define CF_TMS 8.388f
#define CF_ME 32.0f #define CF_ME 32.0f
@@ -34,6 +35,8 @@
/* Per-call input cache. FM_GET_PHIAD's float args land here so the /* Per-call input cache. FM_GET_PHIAD's float args land here so the
* nullary phi getters can read them. */ * nullary phi getters can read them. */
static float s_rpm = 0.0f; static float s_rpm = 0.0f;
static float s_rpm_offset = 0.0f;
static float s_me = 0.0f; static float s_me = 0.0f;
static float s_temp = 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 phi_outputs_t s_phi_out;
static float *s_phiad_out = NULL; 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) ────────────────────────── */ /* ──── Getters (nullary; T06215 vtable shape) ────────────────────────── */
static uint16_t get_rpm(void) { return (uint16_t)(s_rpm * CF_TMS); } 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; * extern float B_PHIAD;
* return (int16_t)(B_PHIAD * CF_KW); * 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 /* Steady-state default: baseline tracks current RPM, so Δ-rpm = 0 and
* the accel-comp branch contributes nothing. Override if modelling * the accel-comp branch contributes nothing. Override if modelling
* transients. */ * 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 uint16_t get_rwc2(void) { return 0; } /* slow timing scratch */
static uint8_t get_reset_gate_0226(void) { return 0xFF; } /* accel-comp enabled */ static uint8_t get_reset_gate_0226(void) { return 0xFF; } /* accel-comp enabled */
static int16_t get_dphi(void) { return 0; } static int16_t get_dphi(void) { return (int16_t)(s_dfi_code); }
static uint8_t get_scratch_0103(void) { return 0xFF; } /* matches sim_t06215_sweep.c */ static uint8_t get_scratch_0103(void) { return 0x78; } /* matches sim_t06215_sweep.c */
static uint8_t get_scratch_0108(void) { return 0xFF; } static uint8_t get_scratch_0108(void) { return 0x9A; }
/* ──── Public API ─────────────────────────────────────────────────────── */ /* ──── 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_inj_qty_demand = get_inj_qty_demand;
s_phi_getters.get_temperature = get_temperature; s_phi_getters.get_temperature = get_temperature;
s_phi_getters.get_angle_dec_cmd = get_angle_dec_cmd; 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_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_dphi = get_dphi;
s_phi_getters.get_scratch_0103 = get_scratch_0103; s_phi_getters.get_scratch_0103 = get_scratch_0103;
s_phi_getters.get_scratch_0108 = get_scratch_0108; 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); phi_tick_1khz(&s_phi_state, &s_phi_cal);
} }
void TW_FM_ISR(uint8_t currentTooth, uint32_t ic){ extern float forceTemp, MT_RPM;
phi_tooth_isr(&s_phi_state, &s_phi_cal, currentTooth, (uint16_t)(ic >> 4)); 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) { 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_me = ME;
s_temp = Temp; s_temp = forceTemp;
phi_service(&s_phi_state, &s_phi_cal, &s_phi_out); phi_service(&s_phi_state, &s_phi_cal, &s_phi_out);
float Z = (float)s_phi_out.angle_accumulator * ANGLE_DEG_PER_RAW; 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 Z = s_me ? Z : 0; // el terrano se apagaba
if (s_phiad_out) *s_phiad_out = Z; if (s_phiad_out) *s_phiad_out = Z;
return Z; return Z;
} }
void TW_midrpm_isr(){
phi_per_cylinder_event(&s_phi_state, &s_phi_cal);
}

11
Core/Src/injection.c
View File

@@ -89,6 +89,9 @@ extern float last_accel;
uint8_t awaitingInj = 0; uint8_t awaitingInj = 0;
float INJ_GET_NOMINAL_EOI(); float INJ_GET_NOMINAL_EOI();
void INJ_UPDATE_PHIAD(){
FM_GET_PHIAD(MT_RPM, ME, Temp);
}
void INJ_UPDATE_ALPHA(){ void INJ_UPDATE_ALPHA(){
/*float rp_sum = MT_RPM + last_MT_RPM; /*float rp_sum = MT_RPM + last_MT_RPM;
float dt = (PHI1 + dFi) / 90 * (1.0f / CYLINDERS) * (120.0f / rp_sum); 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); //FM_GET_PHIAD(MT_RPM, ME, forceTemp);
//correction_eoi_accel = TM_GET_ACCEL_CORRECTION(last_MT_RPM, MT_RPM, TEETH_RPM); //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; float target = INJ_UPDATE_TARGET_EOI() + correction_eoi;
@@ -138,7 +140,7 @@ void INJ_PREPARE_ONCE(){
TIM1->PSC = 20-1; TIM1->PSC = 20-1;
} }
INJ_PREPARE_BIP(0); INJ_PREPARE_BIP(0);
INJ_UPDATE_ALPHA(); //INJ_UPDATE_ALPHA();
INJ_UPDATE_TYPE(); INJ_UPDATE_TYPE();
} }
@@ -148,7 +150,7 @@ void INJ_PREPARE_BIP(uint8_t teeth){
if(teeth < triggerTeeth || !triggerTeeth) if(teeth < triggerTeeth || !triggerTeeth)
{ {
INJ_UPDATE_BOI_TRIGGER(); 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_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; //correction_beta = s_boi_corr_deg + correction_boi_accel_2;
@@ -470,7 +472,8 @@ void SEND1_Handler(
#if defined(T06301) #if defined(T06301)
startupiscar = 1; startupiscar = 1;
#endif #endif
if(!isInjecting && !hasInjectionEnded){ INJ_UPDATE_PHIAD();
if(!isInjecting && !hasInjectionEnded){//
INJ_UPDATE_ALPHA(); INJ_UPDATE_ALPHA();
} }
} }

6
Core/Src/toothed_wheel.c
View File

@@ -195,11 +195,14 @@ void TW_TEETH_CAPTURE(){
} }
INJ_EVAL_END(); INJ_EVAL_END();
if(currentTooth == 13){ if(currentTooth == 13){
RPM_offset_Difference = IC_RPM_Val2 - edgeBuf[currentTooth].ic; RPM_offset_Difference = IC_RPM_Val2 - edgeBuf[currentTooth].ic;
MT_offset_RPM = fclamp(60.0 * (refClock/RPM_offset_Difference) / CYLINDERS, MIN_RPM, 4000); MT_offset_RPM = fclamp(60.0 * (refClock/RPM_offset_Difference) / CYLINDERS, MIN_RPM, 4000);
if(MT_RPM <15){ if(MT_RPM <15){
MT_offset_RPM = MT_RPM; MT_offset_RPM = MT_RPM;
} }
TW_midrpm_isr();
} }
UpdateEdgeBuffer(IC_RPM_Val2, TEETH_RPM, currentTooth, isInjecting); UpdateEdgeBuffer(IC_RPM_Val2, TEETH_RPM, currentTooth, isInjecting);
@@ -233,9 +236,6 @@ void TW_TEETH_CAPTURE(){
if(currentTooth == TW_PERCYL_TEETH - 1){ if(currentTooth == TW_PERCYL_TEETH - 1){
INJ_END(); INJ_END();
} }
TW_FM_ISR(currentTooth,IC_RPM_Val2 );
hasCapturedTeeth = 1; hasCapturedTeeth = 1;