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

424026 working with pwm and phi. Rpm filtered for acceleration compensation still missing. Only using phi_ad and applying our accel comp

Browse Source
This commit is contained in:
LucianoDev
2026-04-30 15:03:57 +02:00
parent 5e20601eeb
commit d95e4e18f1
23 changed files with 2828 additions and 1602 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@@ -38,6 +38,7 @@
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Advance_Control}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Advance_Control}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Phi}&quot;"/>
</option> </option>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.1467004031" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/> <inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.1467004031" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/>
</tool> </tool>
@@ -59,6 +60,7 @@
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Advance_Control}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Advance_Control}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Phi}&quot;"/>
</option> </option>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.264989976" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/> <inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.264989976" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
</tool> </tool>
@@ -85,7 +87,7 @@
</toolChain> </toolChain>
</folderInfo> </folderInfo>
<sourceEntries> <sourceEntries>
<entry excluding="Advance_Control/pwm_504012.c|Advance_Control/cal_tables_rom_504012.c|Advance_Control/pwm_004002.c|Advance_Control/cal_tables_rom_004002.c" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/> <entry excluding="Advance_Control/cal_tables_rom_424026.c|Advance_Control/pwm_504012.c|Advance_Control/cal_tables_rom_504012.c|Advance_Control/pwm_004002.c|Advance_Control/cal_tables_rom_004002.c" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/> <entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
</sourceEntries> </sourceEntries>
</configuration> </configuration>
@@ -125,6 +127,7 @@
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Advance_Control}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Advance_Control}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Phi}&quot;"/>
</option> </option>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.1230182628" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/> <inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.1230182628" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/>
</tool> </tool>
@@ -145,6 +148,7 @@
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Advance_Control}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Advance_Control}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Phi}&quot;"/>
</option> </option>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.1042657329" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/> <inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.1042657329" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
</tool> </tool>
@@ -171,7 +175,7 @@
</toolChain> </toolChain>
</folderInfo> </folderInfo>
<sourceEntries> <sourceEntries>
<entry excluding="Advance_Control/pwm_504012.c|Advance_Control/cal_tables_rom_504012.c|Advance_Control/pwm_004002.c|Advance_Control/cal_tables_rom_004002.c" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/> <entry excluding="Advance_Control/cal_tables_rom_424026.c|Advance_Control/pwm_504012.c|Advance_Control/cal_tables_rom_504012.c|Advance_Control/pwm_004002.c|Advance_Control/cal_tables_rom_004002.c" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/> <entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
</sourceEntries> </sourceEntries>
</configuration> </configuration>

6
Core/Advance_Control/FBKW.c
View File

@@ -97,10 +97,10 @@ static int16_t fbkw_get_b_fb_kw(void *ctx) {
return (int16_t)(B_FB_KW*CF_KW); 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 * TODO: wire when the corresponding CAN signal is exposed. Using
* FBKW_PID_OPEN as a provisional mapping (negative = open-loop sentinel). */ * 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; (void)ctx;
return 0; return 0;
//return (int16_t)(CKP_PULSE_AVAILABLE ? 0 : -1); //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.angle_dec_cmd = fbkw_get_angle_dec_cmd;
fbkw_getters.inj_qty_demand = fbkw_get_inj_qty_demand; fbkw_getters.inj_qty_demand = fbkw_get_inj_qty_demand;
fbkw_getters.b_fb_kw = fbkw_get_b_fb_kw; 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.supply_voltage = fbkw_get_supply_voltage;
fbkw_getters.temperature = fbkw_get_temperature; fbkw_getters.temperature = fbkw_get_temperature;
fbkw_getters.ctx = NULL; fbkw_getters.ctx = NULL;

172
Core/Advance_Control/cal_tables_rom.c
View File

@@ -1,47 +1,58 @@
/** /**
* @file cal_tables_rom.c (families/t06211/compact_src) * @file cal_tables_rom.c (families/T06215/compact_src)
* @brief ROM-decoded t06211 calibration. * @brief ROM-decoded T06215 calibration.
* *
* AUTO-GENERATED by tools/extract_calibration.py * Source ROM: 424026.bin (Bosch P/N 167002X9001494060200)
* Source ROM: rom_eeprom_dump_0000-9FFF_504012.bin * Calibration base: RWA4 = 0x9BD8
* Calibration base (RWA4): 0x9BD8 * Flash anchor: 0x9618
* Flash anchor: 0x7E18 * Generated: 2026-04-29 (hand-extracted via Ghidra MCP + ROM readback)
* Generated: 2026-04-27 12:40:48
* *
* DO NOT EDIT — regenerate with: * Field name → ROM-address binding lives in pwm_addr_map.h. Boot-derived
* python tools/extract_calibration.py --family t06211 * 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" #include "pwm.h"
/* ── Submap x/y arrays ──────────────────────────────────────────────── */ /* ── Submap x/y arrays ──────────────────────────────────────────────── */
static const int16_t setpoint_x[6] = { 8389, 5872, 4614, 2726, 1426, 0 }; static const int16_t setpoint_x[6] = { 8389, 5872, 3775, 2726, 1426, 0 };
static const int16_t setpoint_y[6] = { 1707, 1707, 1707, 939, 427, 427 }; 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_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_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_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_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 }; 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] = { static const int16_t pwm_y_table_rom[90] = {
/* row 0 */ 205, 205, 205, 205, 205, 205, 0, 0, 0, /* row 0 */ 205, 205, 205, 205, 205, 205, 205, 205, 0,
/* row 1 */ 1638, 1392, 1229, 962, 778, 512, 0, 0, 0, /* row 1 */ 1568, 1433, 1355, 1065, 799, 205, 205, 205, 0,
/* row 2 */ 1843, 1577, 1433, 1208, 1003, 758, 0, 0, 0, /* row 2 */ 1716, 1679, 1556, 1290, 983, 205, 205, 205, 0,
/* row 3 */ 2457, 2109, 1986, 1761, 1577, 1229, 287, 0, 0, /* row 3 */ 2219, 2129, 2052, 1925, 1679, 880, 205, 205, 0,
/* row 4 */ 2867, 2744, 2641, 2457, 2314, 2129, 1269, 205, 205, /* row 4 */ 2744, 2580, 2518, 2477, 2355, 1740, 1310, 205, 0,
/* row 5 */ 3686, 3481, 3460, 3378, 3276, 3174, 2907, 1024, 737, /* row 5 */ 3210, 3071, 3030, 3194, 3194, 3018, 2867, 737, 205,
/* row 6 */ 3890, 3870, 3849, 3829, 3808, 3788, 3235, 1310, 1024, /* row 6 */ 3702, 3493, 3403, 3583, 3583, 3493, 3071, 1192, 483,
/* row 7 */ 3890, 3890, 3890, 3890, 3890, 3890, 3890, 2076, 1638, /* row 7 */ 3890, 3890, 3890, 3890, 3890, 3890, 3890, 2076, 1229,
/* row 8 */ 3890, 3890, 3890, 3890, 3890, 3890, 3890, 3890, 3481, /* 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 ────────────────────────────────────────────────────── */ /* ── Descriptors ────────────────────────────────────────────────────── */
@@ -69,70 +80,81 @@ const int16_t *pwm_submap_y_of(uint16_t idx)
{ {
switch (idx) { switch (idx) {
case PWM_SUBMAP_SETPOINT_INTERP: return setpoint_y; case PWM_SUBMAP_SETPOINT_INTERP: return setpoint_y;
case PWM_SUBMAP_PWM_A: return pwm_A_y; case PWM_SUBMAP_PWM_A: return pwm_A_y;
case PWM_SUBMAP_PWM_B: return pwm_B_y; case PWM_SUBMAP_PWM_B: return pwm_B_y;
case PWM_SUBMAP_SHAPE_EVAL: return shape_y; case PWM_SUBMAP_SHAPE_EVAL: return shape_y;
default: return NULL; default: return NULL;
} }
} }
/* ── Scalars ────────────────────────────────────────────────────────── */ /* ── Scalars ────────────────────────────────────────────────────────── */
const pwm_calibration_t pwm_cal_rom = { const pwm_calibration_t pwm_cal_rom = {
.large_pos_error_thresh = 128, /* cal+0x10E */ /* PI controller error-band thresholds */
.large_neg_error_thresh = (int16_t)0xFF00, /* cal+0x110 */ .large_pos_error_thresh = 128, /* cal+0x10E */
.pi_low_clamp = (int16_t)0xFE00, /* cal+0x120 */ .large_neg_error_thresh = (int16_t)0xFF00, /* cal+0x110 = -256 */
.pi_high_clamp = 1707, /* cal+0x124 */ .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 */ /* CAN-decoded setpoint cal constants */
.b_fb_kw_upper_bound = 7680, /* cal+0x004 */ .b_fb_kw_upper_bound = 7680, /* cal+0x004 */
.b_fb_kw_lower_bound = (int16_t)0xFD00, /* cal+0x006 */ .b_fb_kw_lower_bound = (int16_t)0xFD00, /* cal+0x006 */
/* setpoint_offset = cal+0x4c - cal+0x4e = 3499 - 4156 = -657 */ .setpoint_offset = -648, /* cal+0x4c − cal+0x4e */
.setpoint_offset = (int16_t)0xFD6F, .target_5e_min_clamp = (int16_t)0xFE00, /* cal+0x12A */
.target_5e_min_clamp = (int16_t)0xFE00, /* cal+0x122 */ .can_aux_12e_max = 1451, /* cal+0x002 */
.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 */
.pwm_detail_x0 = (int16_t)0x9C40, /* cal+0x0EE */ /* Recovery / sustained-error machinery */
.pwm_detail_x1 = (int16_t)0x8235, /* cal+0x0F0 */ .pi_state_c2_reload = 100, /* cal+0x114 (ROM 0x9CEC = 0x0064) */
.pwm_cached_ptr_0F2 = 5662, /* cal+0x0F2 (legacy scalar) */ .inj_qty_thresh = 96, /* cal+0x116 (ROM 0x9CEE = 0x0060) */
.pwm_cached_ptr_102 = 168, /* cal+0x102 (legacy scalar) */ .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 /* PI runtime-reset values (ROM-extracted from t06215 cal block at
* (4 (lo,hi) pairs) plus halfwidth at cal+0x102. Drives the three- * RWA4=0x9BD8 + offset). Boot multipliers DAT_0410..0416 are
* phase pwm_period slew in FUN_5314 (open-questions §5 closeout). */ * uninitialized RAM (no writers anywhere in ROM), so they default
.pwm_rpm_windows = { 5662, 6795, 8808, 10486, 11954, 13422, 18036, 19713 }, * 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_window_halfwidth = 168,
.pwm_slew_step = 354,
/* pwm_const_104 / pwm_slew_step are the same value (cal+0x104 = 354); .pwm_y_table = pwm_y_table_rom,
* pwm_const_104 retained for backward compat, pwm_slew_step is the .shape_y_table = shape_y_table_rom,
* 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, /* cal+0x154 @ 0x9E28 */ .closed_loop_gain_const = 10,
.shape_y_table = shape_y_table_rom, /* cal+0x15E @ 0x9E20 */
/* CL-correction gain — cached at absolute ROM[0x6056], not cal-relative. */ .pwm_period_min = 33333,
.closed_loop_gain_const = 10, /* ROM[0x6056] */ .pwm_period_max = 40000,
/* PWM period endpoints (alias of pwm_detail_x0/x1) as unsigned. */ .pwm_min = 0x00CD,
.pwm_period_min = 33333, /* cal+0x0F0 */ .pwm_max = 0x0F32,
.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 */
}; };
/* 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 }; const pwm_flash_t pwm_flash_rom = { 0 };

152
Core/Advance_Control/cal_tables_rom_424026.c Normal file
View File

@@ -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 };

611
Core/Advance_Control/pwm.c
View File

@@ -1,22 +1,24 @@
/** /**
* @file pwm.c (families/t06211/compact_src) * @file pwm.c (families/T06215/compact_src)
* @brief Compact single-file implementation of the t06211 PWM control * @brief Compact single-file implementation of the T06215 PWM control
* pipeline. Merges the 10 per-module files from * pipeline.
* families/t06211/src/ into one translation unit.
* *
* Pipeline (t06211 FUN_77b3 @ 0x77b3): * Pipeline (pwm_service @ 0x7780):
* 1. setpoint (FUN_7168) — single-submap RPM-indexed interp * 0. setpoint interp (FUN_7051) — writes pi_high_clamp_ceiling (DAT_0344)
* 2. supervisor (FUN_7beb) — reset + counter + error + clamp * 1. supervisor (s_supervisor) — reset + counter + error + clamp
* 3. publish_cl (FUN_7cd8) — calls cl_correction, publishes est_angle * 2. publish_cl (s_publish_cl) — calls cl_correction, publishes est_angle
* 4. pi_controller (FUN_67c4) — open/closed-loop branch * 3. pi_update (s_pi_update @ 0x542f) — open/closed-loop PI body
* 5. pwm_output (FUN_5314) — eval+eval+combine+saturate+HW shadow * 4. pwm_output (s_pwm_output) — eval+eval+combine+saturate+HW shadow
* *
* Per-block address citations follow the families/t06211/src/ per-module * The PI block (s_pi_update + s_recovery) was re-translated 1:1 from the
* ports verbatim; see those files for expanded commentary. * 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" #include "pwm.h"
/* Forward decls for file-static helpers. */ /* Forward decls. */
static void s_eval_submap(const pwm_submap_descr_t *d, static void s_eval_submap(const pwm_submap_descr_t *d,
pwm_interp_slot_t *slot); pwm_interp_slot_t *slot);
static int16_t s_combine(const int16_t *y_base, 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); const pwm_calibration_t *cal);
static void s_pwm_output (pwm_runtime_t *rt, static void s_pwm_output (pwm_runtime_t *rt,
const pwm_calibration_t *cal); const pwm_calibration_t *cal);
static void s_recovery (pwm_runtime_t *rt,
const pwm_calibration_t *cal);
/* ═════════════════════════════════════════════════════════════════════ /* ═════════════════════════════════════════════════════════════════════
* Init / binding * 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, static void apply_cal(pwm_runtime_t *rt,
const pwm_calibration_t *cal) const pwm_calibration_t *cal)
{ {
/* Mirrors family-1's pwm_init copy of cal→runtime statics. */ /* All boot-derived constants come from cal — no literals here. */
rt->setpoint_offset_150 = cal->setpoint_offset; 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, 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_flash_t *flash,
const pwm_input_getters_t *getters) const pwm_input_getters_t *getters)
{ {
(void)flash; /* family-1 API parity; t06211 keeps Y-tables in cal */ (void)flash;
runtime_reset(rt); runtime_reset(rt);
rt->bound_cal = cal; rt->bound_cal = cal;
rt->bound_getters = getters; rt->bound_getters = getters;
@@ -97,22 +88,18 @@ static void read_inputs(pwm_runtime_t *rt)
void *ctx = g->ctx; void *ctx = g->ctx;
rt->inputs.ckp_in = g->ckp_in (ctx); rt->inputs.ckp_in = g->ckp_in (ctx);
rt->inputs.rpm = g->rpm (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.inj_qty_demand = g->inj_qty_demand(ctx);
rt->inputs.b_fb_kw = g->b_fb_kw (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.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; rt->can_raw_b_fb_kw = rt->inputs.b_fb_kw;
} }
/* ═════════════════════════════════════════════════════════════════════ /* ═════════════════════════════════════════════════════════════════════
* Interpolation — FUN_7168 (0x7168-0x71d7), fingerprint #3 * Interpolation (FUN_7168, fingerprint #3)
* Raw helper; also drives the setpoint stage via descriptor.
* ═════════════════════════════════════════════════════════════════════ */ * ═════════════════════════════════════════════════════════════════════ */
int16_t pwm_interp_lookup(const int16_t *x, const int16_t *y, 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); int32_t prod = MUL_S16(num, dy);
int16_t quot = (int16_t)(prod / (int32_t)dx); 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]); return (int16_t)(quot + y[k]);
} }
/* ═════════════════════════════════════════════════════════════════════ /* ═════════════════════════════════════════════════════════════════════
* Submap eval / bilinear combine / refine — t06211 FUN_6fb8 / FUN_7035 / * Submap eval / bilinear combine / refine
* FUN_7014. Scratch layout matches the family-1 ROM convention.
* ═════════════════════════════════════════════════════════════════════ */ * ═════════════════════════════════════════════════════════════════════ */
static void s_eval_submap(const pwm_submap_descr_t *d, 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]) { if (k == 1u && input >= d->x[0]) {
/* Upper-clamp sentinel [_, 2, 2, 2] */
slot->x_interval = 2; slot->x_interval = 2;
slot->x_offset = 2; slot->x_offset = 2;
slot->y_byte_off = 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)); 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, static int16_t s_interp_descr(const pwm_submap_descr_t *d,
const int16_t *y_array) 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) 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 = const pwm_submap_descr_t *d =
&pwm_submap_descrs[PWM_SUBMAP_SETPOINT_INTERP]; &pwm_submap_descrs[PWM_SUBMAP_SETPOINT_INTERP];
const int16_t *y = pwm_submap_y_of(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) * Stage 0b — CAN-decoded setpoint (FUN_64c3 family)
* 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).
* ═════════════════════════════════════════════════════════════════════ */ * ═════════════════════════════════════════════════════════════════════ */
static void s_setpoint_can_decode(pwm_runtime_t *rt, static void s_setpoint_can_decode(pwm_runtime_t *rt,
const pwm_calibration_t *cal) const pwm_calibration_t *cal)
{ {
int16_t raw = rt->can_raw_b_fb_kw; 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_upper_bound) return;
if (raw < cal->b_fb_kw_lower_bound) return; if (raw < cal->b_fb_kw_lower_bound) return;
/* [0x64dd] half = raw >> 1 (SHRA — sign-extending) */
int16_t half = shra16(raw, 1); int16_t half = shra16(raw, 1);
rt->can_half_12a = half; rt->can_half_12a = half;
/* [0x64e5-0x64ea] result = half + RAM[0x150] + RW42 */ int16_t result = (int16_t)(half + rt->setpoint_offset + rt->rw42_state);
int16_t result = (int16_t)(half + rt->setpoint_offset_150 + rt->rw42_state);
/* [0x64ed-0x64f9] Lower clamp */
if (result < cal->target_5e_min_clamp) { if (result < cal->target_5e_min_clamp) {
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) static void s_supervisor(pwm_runtime_t *rt)
{ {
/* Reset-flag branch (0x7beb-0x7c0c) */
if (rt->reset_flag == 1u) { if (rt->reset_flag == 1u) {
rt->reset_flag = 0u; rt->reset_flag = 0u;
rt->cl_enable_counter = 0u; rt->cl_enable_counter = 0u;
rt->supervisor_state_17e = 0; rt->supervisor_state = 0;
rt->b_fb_kw_baseline = rt->inputs.b_fb_kw; rt->pi_integ_hi_snapshot = rt->pi_integ_hi; /* dead-store parity */
} else { } else {
/* Counter tick (0x7c11-0x7c1b) */
rt->cl_enable_counter = (uint16_t)(rt->cl_enable_counter + 1u); 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); 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; rt->angle_error_raw = error;
/* Ceiling clamp (0x7c30-0x7c41) if (error > rt->pi_high_clamp_ceiling) {
* Disasm: CMP RW1C, DAT_0336 — clamp uses RPM-derived ceiling. */ rt->angle_error_raw = rt->pi_high_clamp_ceiling;
if (error > rt->target_336) {
rt->angle_error_raw = rt->target_336;
} }
} }
/* ═════════════════════════════════════════════════════════════════════ /* ═════════════════════════════════════════════════════════════════════
* Stage 3a — Closed-loop correction (FUN_5f1f @ 0x5f1f-0x5f66) * Stage 2a — Closed-loop correction
* Fingerprint #10 bit-for-bit match to family-1 algorithm.
* ═════════════════════════════════════════════════════════════════════ */ * ═════════════════════════════════════════════════════════════════════ */
static void s_cl_correct(pwm_runtime_t *rt, 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; int16_t correction;
/* Gate on low byte of cl_enable_counter (0x5f1f-0x5f24). */
if ((rt->cl_enable_counter & 0xFFu) == 0u) { if ((rt->cl_enable_counter & 0xFFu) == 0u) {
correction = 0; correction = 0;
} else { } else {
@@ -326,13 +313,13 @@ static void s_cl_correct(pwm_runtime_t *rt,
: (int16_t)0; : (int16_t)0;
} }
rt->cl_correction_raw = correction; rt->cl_correction_raw = correction;
rt->supervisor_state_17e = (int16_t)(rt->supervisor_state_17e + correction); rt->supervisor_state = (int16_t)(rt->supervisor_state + correction);
rt->angle_offset = shra16(rt->supervisor_state_17e, 4u); 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, 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 /* s_recovery @ 0x53a2.
* default family's `s_recovery` / `fast_recovery`; same name across *
* variants for cross-family alignment. Disasm/source mapping (audit * Disasm:
* table — keep the cooldown gate and gated-increment branches in this * 53a2 LDBZE RW1C, pi_shape_flag ; zero-extended into 16-bit
* exact order): * 53a7 SHL RW1C, #2 ; <<2 (in 16-bit reg)
* 66ad-66b2 CMPB pi_shape_flag, pi_flag_c6 ; JNE LAB_671d * 53aa LDBZE RW1E, pi_shape_flag
* 66b9-66be CMP pi_state_118, [0x02c0]=cal+0x112 ; JNC LAB_66e6 * 53af AND RW1C, RW1E ; (flag<<2) & flag
* JNC ⇒ counter < threshold ⇒ gated-increment * 53b2 CMP RW1C, #0x30
* fall-through ⇒ counter ≥ threshold ⇒ latch+reset * 53b6 JNH LAB_5421 ; <= 0x30 → "no sustained" path
* 66c0-66e5 latch bit0 ; counter = 0 ; pi_state_c2 ← cal+0x114 * ; --- Sustained large-error detected (this+previous cycle bit4 or bit5) ---
* 66e6-66f4 CMP rpm, [cal+0x11E] ; JLE exit * 53b8 LD RW1C, pi_state_118
* 66f6-6705 JBS bit4 / JBS bit5 — exit * 53bd CMP RW1C, RAM[0x027a] ; RAM[0x027a] = cal+0x112 boot-cached
* 6706-670b CMP ZR, pi_state_c2 ; JNE exit * 53c2 JNC LAB_53ea ; counter < threshold → gated-increment
* 670d-6717 pi_state_118 += 1 * ; --- Latch path: counter saturated ---
* 671d-6725 JBS bit0 exit ; pi_state_118 = 0 * 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, static void s_recovery(pwm_runtime_t *rt,
const pwm_calibration_t *cal) 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) { if ((rt->system_flags_110 & 0x01u) == 0u) {
rt->pi_state_118 = 0; 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, static void s_pi_update(pwm_runtime_t *rt,
const pwm_calibration_t *cal) const pwm_calibration_t *cal)
{ {
/* PI CL-gate threshold = cached ROM[0x605c]. Disasm site: /* ── Phase A — Open-loop gate [0x542f-0x5489] ──
* FUN_67c4:67d9 `CMP RW1C, DAT_605c` (literal 0x01A4 = 420). In the * Disasm: bit-test R110.5; CMP RW130 < 0; CMP rpm < flash[0x605c]. */
* 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;
bool open_loop = false; bool open_loop = false;
if ((rt->system_flags_110 & 0x20u) != 0u) open_loop = true; if ((rt->system_flags_110 & 0x20u) != 0u) open_loop = true;
else if (rt->inputs.state_130 < 0) open_loop = true; else if (rt->inputs.cl_gate_input < 0) open_loop = true;
else if ((int16_t)rt->inputs.rpm < rpm_floor) open_loop = true; else if ((int16_t)rt->inputs.rpm < cal->pi_cl_rpm_floor) open_loop = true;
if (open_loop) { if (open_loop) {
rt->pi_open_loop_flag = 0; /* 0x544b-0x5453: pi_shape_flag |= 0x01 */
/* Disasm 0x67e5: LD RW46, RW5E — open-loop assigns the CAN-decoded rt->pi_shape_flag = (uint8_t)(rt->pi_shape_flag | 0x01u);
* primary setpoint, NOT the RPM-derived ceiling. */ /* 0x5458: DAT_0274 = RW5E */
rt->active_request = rt->target_5e; rt->pi_preclamp_out = rt->target_5e;
/* cal+0x120 is a LOWER bound here (disasm: JLT clamps when /* 0x5468-0x5482: if (RW5E < cal[0x128]) DAT_0274 = cal[0x128] */
* active_request < limit). Same constant also serves as the if (rt->target_5e < cal->pi_low_clamp) {
* Block-4 low clamp, hence `pi_low_clamp`. See rt->pi_preclamp_out = cal->pi_low_clamp;
* 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;
} }
goto final_flag; goto trailing_rotate;
} }
/* Disasm 0x680a: SUB RW1C, RW5E, DAT_02cc — CL error uses target. */ /* ── Phase B — Error-band classify [0x5489-0x554e] ──
rt->angle_error_pi = (int16_t)(rt->target_5e - rt->estimated_angle); * Disasm 0x5489: SUB RW1C, RW5E, DAT_02cc → DAT_0278. */
int16_t err = rt->angle_error_pi; int16_t err = (int16_t)(rt->target_5e - rt->estimated_angle);
rt->angle_error_pi = err;
if (err > cal->large_pos_error_thresh) { 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); s_recovery(rt, cal);
} else if (err < cal->large_neg_error_thresh) { } else if (err < cal->large_neg_error_thresh) {
rt->pi_shape_flag = 0x01; /* 0x54cd-0x54d8: flag = (flag | 0x20) & 0xEF */
if (rt->inputs.inj_qty_demand <= cal->pi_thresh_116) { rt->pi_shape_flag = (uint8_t)((rt->pi_shape_flag | 0x20u) & 0xEFu);
rt->pi_state_118 = 0; /* 0x54dd-0x54f9: JH check is unsigned; matches CMP RW44, cal[0x116]. */
} else { if ((uint16_t)rt->inputs.inj_qty_demand > (uint16_t)cal->inj_qty_thresh) {
s_recovery(rt, cal); s_recovery(rt, cal);
} else {
rt->pi_state_118 = 0;
} }
} else { } else {
rt->pi_shape_flag = 0x20; /* 0x54fb-0x554e: flag &= 0xCF; decrement c2 (clear latch when 0); decrement 118 */
if (rt->pi_state_c2 > 0) { 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); rt->pi_state_c2 = (int16_t)(rt->pi_state_c2 - 1);
} }
if (rt->pi_state_c2 == 0) { 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] ── /* ── Phase C — P-shape into pi_p_term / pi_p_gain_active [0x554e-0x560c] ── */
* Strict > / < per disasm JLE 68c9 / JGE 68ed. Bounds at int32_t pterm32; /* 32-bit accumulator for the SHRAL (>>8) */
* 0x0450/0x0452 are set once at boot by FUN_76aa — see if (err > rt->p_shape_bound_pos) {
* docs/open-questions.md §2 closeout. */ /* 0x555a-0x558f: large-positive arm. */
if (err > rt->pi_error_bound_pos) { rt->pi_shape_flag = (uint8_t)(rt->pi_shape_flag | 0x02u);
rt->active_request = cal->pi_high_clamp; rt->pi_p_gain_active = rt->integ_step_large_pos; /* DAT_0286 = DAT_0282 */
rt->pi_flag_338 = 0x01; int32_t slope_part = MUL_S16((int16_t)(err - rt->p_shape_bound_pos),
} else if (err < rt->pi_error_bound_neg) { rt->p_slope_large_pos);
rt->active_request = cal->pi_low_clamp; int32_t anchor_part = MUL_S16(rt->p_gain_normal, rt->p_shape_bound_pos);
rt->pi_flag_338 = 0x01; 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 { } 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: /* ── Phase D — Integrator gate [0x560c-0x5679] ──
rt->pi_open_loop_flag = 0x01; * Disasm 0x560c-0x5611: if pi_shape_flag.bit0 set (open-loop, but we
rt->pi_flag_c6 = rt->pi_shape_flag; * 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, static void s_pwm_output(pwm_runtime_t *rt,
const pwm_calibration_t *cal) const pwm_calibration_t *cal)
{ {
/* A. eval pwm_A */
s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_PWM_A], s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_PWM_A],
&rt->pwm_slot_a); &rt->pwm_slot_a);
/* B. eval pwm_B */
s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_PWM_B], s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_PWM_B],
&rt->pwm_slot_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); int16_t duty = s_combine(cal->pwm_y_table, &rt->pwm_slot_a, &rt->pwm_slot_b);
rt->pwm_duty = (uint16_t)duty; 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; if (rt->pwm_duty < 0x29u) rt->pwm_duty_range_flag = 1u;
else if (rt->pwm_duty > 0xFD7u) rt->pwm_duty_range_flag = 2u; else if (rt->pwm_duty > 0xFD7u) rt->pwm_duty_range_flag = 2u;
else rt->pwm_duty_range_flag = 0u; 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]; uint16_t *pwm_period = (uint16_t *)&rt->pwm_shape_state[0];
const int16_t rpm_s = (int16_t)rt->inputs.rpm; const int16_t rpm_s = (int16_t)rt->inputs.rpm;
const int16_t halfwidth = cal->pwm_window_halfwidth; const int16_t halfwidth = cal->pwm_window_halfwidth;
const int16_t step_mag = cal->pwm_slew_step; const int16_t step_mag = cal->pwm_slew_step;
/* Phase 1 — strict-band test [0x53be-0x542d] */
int slew_set = 0; int slew_set = 0;
for (int bi = 0; bi < 4; bi++) { for (int bi = 0; bi < 4; bi++) {
int16_t lo = cal->pwm_rpm_windows[bi * 2]; 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) { if (!slew_set) {
/* Phase 2 — fine hysteresis margin [0x5434-0x54fe] */
int phase3 = 1; int phase3 = 1;
for (int bi = 0; bi < 4; bi++) { for (int bi = 0; bi < 4; bi++) {
int16_t lo = cal->pwm_rpm_windows[bi * 2]; int16_t lo = cal->pwm_rpm_windows[bi * 2];
int16_t hi = cal->pwm_rpm_windows[bi * 2 + 1]; int16_t hi = cal->pwm_rpm_windows[bi * 2 + 1];
if (rpm_s <= (int16_t)(lo - halfwidth)) { if (rpm_s <= (int16_t)(lo - halfwidth)) {
continue; /* below extended low: try next band */ continue;
} }
if (rpm_s < (int16_t)(hi + halfwidth)) { if (rpm_s < (int16_t)(hi + halfwidth)) {
phase3 = 0; /* HOLD */ phase3 = 0;
break; break;
} }
/* rpm_s >= hi + halfwidth: advance to next band */
} }
if (phase3 && *pwm_period < cal->pwm_period_max) { if (phase3 && *pwm_period < cal->pwm_period_max) {
/* Phase 3 — deep-out negative slew [0x54ff-0x551b] */
rt->pwm_slew_increment = (int16_t)(-step_mag); 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; int32_t period_next = (int32_t)*pwm_period - (int32_t)rt->pwm_slew_increment;
if (period_next < 0) period_next = 0; if (period_next < 0) period_next = 0;
if (period_next > 0xFFFF) period_next = 0xFFFF; if (period_next > 0xFFFF) period_next = 0xFFFF;
*pwm_period = (uint16_t)period_next; *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_min) *pwm_period = cal->pwm_period_min;
if (*pwm_period > cal->pwm_period_max) *pwm_period = cal->pwm_period_max; if (*pwm_period > cal->pwm_period_max) *pwm_period = cal->pwm_period_max;
/* E2. Shape detail [0x557a-0x5608] */
pwm_interp_slot_t shape_slot; pwm_interp_slot_t shape_slot;
s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_SHAPE_EVAL], s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_SHAPE_EVAL],
&shape_slot); &shape_slot);
@@ -667,11 +617,6 @@ static void s_pwm_output(pwm_runtime_t *rt,
if (shape_height > 0x199) shape_height = 0x199; if (shape_height > 0x199) shape_height = 0x199;
rt->pwm_shape_state[5] = shape_height; 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 int16_t slope = (int16_t)(((int32_t)cal->pwm_period_max
- (int32_t)cal->pwm_period_min) >> 8); - (int32_t)cal->pwm_period_min) >> 8);
int16_t pmx_delta = (int16_t)(((int32_t)cal->pwm_period_max 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; : 0;
int32_t duty_new = (int32_t)rt->pwm_duty + shape_add; 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_min) duty_new = (int32_t)cal->pwm_min;
if (duty_new > (int32_t)cal->pwm_max) duty_new = (int32_t)cal->pwm_max; if (duty_new > (int32_t)cal->pwm_max) duty_new = (int32_t)cal->pwm_max;
rt->pwm_duty = (uint16_t)duty_new; 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; uint32_t on_product = (uint32_t)(*pwm_period) * (uint32_t)rt->pwm_duty;
rt->pwm_on_time = (uint16_t)(on_product / 0xFFFu); rt->pwm_on_time = (uint16_t)(on_product / 0xFFFu);
rt->pwm_off_time = (uint16_t)(*pwm_period - rt->pwm_on_time); 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) * Bypass-PI LUT helper
* 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.
* ═════════════════════════════════════════════════════════════════════ */ * ═════════════════════════════════════════════════════════════════════ */
uint16_t pwm_lut_duty(const pwm_calibration_t *cal, 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 * Public entry — pwm_service (mirrors pwm_service @ 0x7780)
* Mirrors FUN_77b3 (0x77b3-0x77d8) — 5-call linear dispatcher.
* ═════════════════════════════════════════════════════════════════════ */ * ═════════════════════════════════════════════════════════════════════ */
void pwm_service(pwm_runtime_t *rt) 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; const pwm_calibration_t *cal = rt->bound_cal;
s_setpoint(rt); /* [0x77b3-0x77c7] target_336 = pwm_A interp(rpm) */ s_setpoint(rt); /* writes pi_high_clamp_ceiling */
s_setpoint_can_decode(rt, cal); /* CAN-decoded target (FUN_64c3 in ROM) */ s_setpoint_can_decode(rt, cal);
s_supervisor(rt); /* [0x77cc] */ s_supervisor(rt);
s_publish_cl(rt, cal);/* [0x77cf] */ s_publish_cl(rt, cal);
s_pi_update(rt, cal); /* [0x77d2] */ s_pi_update(rt, cal);
s_pwm_output(rt, cal);/* [0x77d5] */ s_pwm_output(rt, cal);
} }

387
Core/Advance_Control/pwm.h
View File

@@ -1,21 +1,20 @@
/** /**
* @file pwm.h (families/t06211/compact_src) * @file pwm.h (families/T06215/compact_src)
* @brief Compact single-header API for the t06211 PWM controller. * @brief Compact single-header API for the T06215 PWM controller.
* *
* Mirrors the public shape of the default family's `compact_src/pwm.h` * Variant note: this header was originally forked from t06211 and carried
* but tailored to t06211's 5-stage pipeline (no standalone max_cl_error * runtime/cal field names that embedded RAM addresses (e.g. `pi_b4_state`,
* lookup, single-submap setpoint, factor+offset target compute). * `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_init() — one-time setup
* pwm_service() — per-cycle update (pulls inputs via getters, * pwm_service() — per-cycle update (5-stage pipeline)
* runs the 5-stage pipeline, writes rt outputs)
* *
* External inputs arrive through a getter vtable; each callback returns * External inputs arrive through a getter vtable; callbacks return one
* one signal in native PWM units. * signal in native PWM units.
*
* Every arithmetic choice mirrors the MCS-96 assembly; see
* families/t06211/src/ for per-stage commentary.
*/ */
#ifndef PWM_H #ifndef PWM_H
#define PWM_H #define PWM_H
@@ -44,49 +43,37 @@ static inline int32_t shra32(int32_t v, unsigned n) {
/* ══════════════════════════════════════════════════════════════════════ /* ══════════════════════════════════════════════════════════════════════
* 1D INTERPOLATION SLOT * 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 { typedef struct pwm_interp_slot {
int16_t row_stride; /* count * 2 (byte stride for Y-table rows) */ int16_t row_stride;
int16_t x_interval; /* x[k-1] - x[k] */ int16_t x_interval;
int16_t x_offset; /* input - x[k] */ int16_t x_offset;
int16_t y_byte_off; /* k * 2 */ int16_t y_byte_off;
} pwm_interp_slot_t; } pwm_interp_slot_t;
/* ══════════════════════════════════════════════════════════════════════ /* ══════════════════════════════════════════════════════════════════════
* EXTERNAL INPUTS * 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 { typedef struct pwm_inputs {
int16_t ckp_in; /* 0x02f8 — sensed position */ int16_t ckp_in; /* sensed position */
uint16_t rpm; /* 0x0040 — engine RPM */ uint16_t rpm; /* engine RPM */
int16_t angle_dec_cmd; /* 0x0042 — accepted for family-1 API parity; unused here */ int16_t angle_dec_cmd; /* accepted for cross-family API parity; unused here */
int16_t inj_qty_demand; /* 0x0044 — CAN: inj quantity demand (PI large-neg gate) */ int16_t inj_qty_demand; /* CAN: injection-quantity demand */
int16_t b_fb_kw; /* CAN: plunger feedback baseline. Gets copied into int16_t b_fb_kw; /* CAN: plunger feedback baseline (drives target) */
can_raw_b_fb_kw and decoded by s_setpoint_can_decode int16_t cl_gate_input; /* CAN: open/closed-loop discriminant (was state_130) */
(port of FUN_64c3) to produce target. */ uint16_t supply_voltage; /* shape_eval submap input */
int16_t state_130; /* 0x0130 — CAN: open/closed-loop discriminant */ int16_t temperature; /* accepted for cross-family API parity; unused here */
uint16_t supply_voltage; /* 0x0142 — shape_eval submap input */
int16_t temperature; /* 0x0146 — accepted for family-1 API parity; unused here */
} pwm_inputs_t; } 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 { typedef struct pwm_input_getters {
int16_t (*ckp_in) (void *ctx); int16_t (*ckp_in) (void *ctx);
uint16_t (*rpm) (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 (*inj_qty_demand)(void *ctx);
int16_t (*b_fb_kw) (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); uint16_t (*supply_voltage)(void *ctx);
int16_t (*temperature) (void *ctx); /* accepted; unused by t06211 */ int16_t (*temperature) (void *ctx);
void *ctx; void *ctx;
} pwm_input_getters_t; } pwm_input_getters_t;
@@ -95,128 +82,95 @@ typedef struct pwm_submap_descr pwm_submap_descr_t;
/* ══════════════════════════════════════════════════════════════════════ /* ══════════════════════════════════════════════════════════════════════
* RUNTIME STATE * RUNTIME STATE
* RAM-address comments reference the t06211 MCS-96 map (see * Field names are purely semantic. ROM-address bindings live in
* families/t06211/docs/variable-glossary.md). * pwm_addr_map.h (documentation-only).
* ══════════════════════════════════════════════════════════════════════ */ * ══════════════════════════════════════════════════════════════════════ */
typedef struct pwm_runtime { typedef struct pwm_runtime {
/* External inputs — refreshed each cycle from getters */ /* External inputs — refreshed each cycle from getters */
pwm_inputs_t inputs; pwm_inputs_t inputs;
/* Async flags. /* Async flags. */
* reset_flag — set on reset edge; supervisor clears. uint8_t reset_flag;
* system_flags_110 — bit 5 forces open-loop; bit 0 is fast-recovery latch. uint8_t system_flags_110; /* bit 5 = OL force; bit 0 = recovery latch */
*/
uint8_t reset_flag; /* 0x002e */
uint8_t system_flags_110; /* 0x0110 */
/* ── Dual-target setpoint architecture ── */ /* ── Setpoint architecture ── */
/* target (RW5E = RAM[0x005e]): PRIMARY CAN-driven setpoint /* target_5e: PRIMARY CAN-decoded setpoint (RW5E in t06215). */
* computed by FUN_64c3 (called from CAN parser FUN_6192). Drives int16_t target_5e;
* the PI controller in both open-loop (FUN_67c4:67e5) and /* pi_high_clamp_ceiling: SECONDARY RPM-derived ceiling — written each
* closed-loop (FUN_67c4:680a) paths, and supervisor error compute * cycle by pwm_service from the setpoint submap interp; consumed as
* (FUN_7beb:7c20). Formula: raw/2 + setpoint_offset_150 + rw42_state, * the PI integrator output upper-clamp inside s_pi_update and the
* clamped >= cal->target_5e_min_clamp. */ * supervisor error ceiling. (DAT_0344 in t06215; was misnamed
int16_t target_5e; /* 0x005e — primary CAN-decoded setpoint */ * target_336 in the original fork from t06211.) */
/* target_336 (DAT_0336): SECONDARY RPM-derived ceiling computed by int16_t pi_high_clamp_ceiling;
* 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 */
/* ── CL correction (RAM-layout identical to family 1) ── */ /* CAN-staging buffers — written before pwm_service so s_setpoint_can_decode
int16_t cl_correction_raw; /* 0x0176 */ * can transform them into target_5e. */
int16_t angle_offset; /* 0x017c */ int16_t can_raw_b_fb_kw;
int16_t supervisor_state_17e; /* 0x017e — CL accumulator */ int16_t can_aux_12e;
int16_t pos_error_normalizer; /* 0x0332 */ int16_t can_half_12a;
int16_t neg_error_normalizer; /* 0x0334 */ 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 ── */ /* ── Publish + PI ── */
int16_t estimated_angle; /* 0x02cc */ int16_t estimated_angle;
int16_t angle_error_pi; /* 0x02be */ int16_t angle_error_pi; /* target_5e − estimated_angle (DAT_0278) */
int16_t active_request; /* 0x0046 (RW46) — PI output / PWM feed-forward */ int16_t active_request; /* PI output = pre-clamp output (RW46 + DAT_0274) */
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 */
/* PI integrator at {RAM[0x02b4]:RAM[0x02b6]} — was misnamed "b_fb_kw" /* PI flag byte (DAT_028c). Bit layout (s_pi_update @ 0x542f):
* across the original glossary. Disasm `disasm_nav rw 0x02b4` shows * bit 0 = open-loop indicator (consumed by integrator gate)
* only internal writers (FUN_672b, FUN_76aa, FUN_7c85), confirming * bit 1 = P-shape large-error arm taken
* this is purely a PI integrator state, not an external/CAN input. * bit 2 = output clamped to pi_low_clamp this cycle
* Treated as a signed 32-bit pair: hi at 0x02b4 (the value used in * bit 3 = output clamped to pi_high_clamp_ceiling this cycle
* active_request = comp + pi_b4_state), lo at 0x02b6. */ * bit 4 = error > large_pos_error_thresh this cycle
int16_t pi_b4_state; /* 0x02b4 — integrator high word */ * bit 5 = error < large_neg_error_thresh this cycle
int16_t pi_b6_state; /* 0x02b6 — integrator low word */ * 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 /* PI working state used by s_pi_update */
* with optional <<4 byte trims at RAM[0x0410]/[0x0412] (no writers, int16_t pi_p_term; /* DAT_0276 — output of P-shape segment */
* default 0). Defaults: 0x0454=+480 (post-scale), 0x0456=+256 (step). */ int16_t pi_p_gain_active; /* DAT_0286 — selected gain for integrator step */
int16_t pi_post_scale_454; /* 0x0454 — multiplier in comp = (err*scale)>>8 */ int16_t pi_preclamp_out; /* DAT_0274 — pre-publish active_request */
int16_t pi_integ_step_456; /* 0x0456 — multiplier in FUN_7c85 integrator step */
/* PI integ gain — boot-set by FUN_76aa from cal+0x11C (byte, clamped /* PI integrator pair {hi:lo} = {DAT_028a:DAT_0288} */
* ≤15). Used in the "reset" branch as: pi_b4_state = (err*gain)>>4 + ckp. int16_t pi_integ_lo;
* The runtime field formerly named "pwm_period" was a holdover from int16_t pi_integ_hi;
* 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) */
/* Anti-windup flag set by FUN_672b clamps; consulted by FUN_7c85 /* P-shape segment bounds (boot-init from cal+0x10A/0x10C). */
* to gate integration direction. Values: 0 (in-range), 1 (clamped int16_t p_shape_bound_pos;
* low), 2 (clamped high). */ int16_t p_shape_bound_neg;
uint8_t pi_flag_c7; /* 0x02c7 */
/* 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 ── */ /* ── PWM output ── */
uint16_t pwm_duty; /* 0x02d2 */ uint16_t pwm_duty;
uint16_t pwm_on_time; /* 0x02ce */ uint16_t pwm_on_time;
uint16_t pwm_off_time; /* 0x02d0 */ uint16_t pwm_off_time;
/* 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_period; uint16_t pwm_period;
uint8_t pwm_duty_range_flag; /* 0x00d1 */ uint8_t pwm_duty_range_flag;
pwm_interp_slot_t pwm_slot_a; /* 0x02d4 */ pwm_interp_slot_t pwm_slot_a;
pwm_interp_slot_t pwm_slot_b; /* 0x02dc */ pwm_interp_slot_t pwm_slot_b;
/* pwm_shape_state[6] mirrors RAM[0x02e4..0x02ee]: int16_t pwm_shape_state[6];
* [0] pwm_period working value (RAM 0x02e4) int16_t pwm_slew_increment;
* [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 */
/* ── Bindings (set once by pwm_init) ── */ /* ── Bindings (set once by pwm_init) ── */
const pwm_calibration_t *bound_cal; const pwm_calibration_t *bound_cal;
@@ -225,80 +179,73 @@ typedef struct pwm_runtime {
/* ── Calibration (decoded ROM values) ───────────────────────────────── */ /* ── Calibration (decoded ROM values) ───────────────────────────────── */
struct pwm_calibration { 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_pos_error_thresh; /* cal+0x10E */
int16_t large_neg_error_thresh; /* cal+0x110 */ 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_low_clamp; /* cal+0x128 — output low clamp + open-loop lower bound */
int16_t pi_high_clamp; /* cal+0x124 = +1707 — Block-4 high clamp */ int16_t pi_high_clamp; /* fallback upper clamp; runtime uses pi_high_clamp_ceiling */
/* CAN-decoded setpoint (FUN_64c3) cal constants */ /* CAN-decoded setpoint (FUN_64c3 family) cal constants */
int16_t b_fb_kw_upper_bound; /* cal+0x004 = +7680 — raw b_fb_kw upper sanity bound */ int16_t b_fb_kw_upper_bound; /* cal+0x004 */
int16_t b_fb_kw_lower_bound; /* cal+0x006 = -768 — raw b_fb_kw lower sanity bound */ 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. /* Recovery / sustained-error machinery */
* Family-1 analog: CAL+0x0052 - CAL+0x0054 (compact_src/pwm.h:210). int16_t pi_state_c2_reload; /* cal+0x114 — reload value for pi_state_c2 on latch */
* For t06211, FUN_6b7b computes the equivalent as cal+0x4c - cal+0x4e int16_t inj_qty_thresh; /* cal+0x116 — inj-qty threshold for recovery vs reset */
* (= 3499 - 4156 = -657) at boot and stores at RAM[0x150]. int16_t pi_sat_count_threshold; /* cal+0x112 — recovery counter latch threshold */
* Copied into runtime.setpoint_offset_150 by runtime_reset. */ int16_t rpm_threshold_recovery; /* cal+0x126 — s_recovery RPM gate (was wrongly cal+0x11E in t06211 idiom) */
int16_t setpoint_offset; /* = cal+0x4c - cal+0x4e */ int16_t pi_cl_rpm_floor; /* flash[0x605C] — s_pi_update OL gate threshold */
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;
int16_t pwm_detail_x0; /* cal+0x0EE */ /* PI runtime-reset values (boot-derived in ROM; cal-resident here so
int16_t pwm_detail_x1; /* cal+0x0F0 */ * each variant carries its own values without code edits). */
int16_t pwm_cached_ptr_0F2; /* cal+0x0F2 — first RPM-window breakpoint (legacy scalar) */ int16_t init_p_shape_bound_pos; /* cal+0x10A — copied to rt->p_shape_bound_pos */
int16_t pwm_cached_ptr_102; /* cal+0x102 — window halfwidth (legacy scalar) */ int16_t init_p_shape_bound_neg; /* cal+0x10C — copied to rt->p_shape_bound_neg */
int16_t pwm_const_104; /* cal+0x104 */ 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). /* CL correction normalizers (RAM in ROM, cal-resident here for parity). */
* Eight breakpoints at cal+0xF2 define four bands (lo,hi); the int16_t init_pos_error_normalizer;
* cal+0x102 halfwidth adds hysteresis. RPM INSIDE any band drives int16_t init_neg_error_normalizer;
* 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 */
const int16_t *pwm_y_table; /* cal+0x154 (indirect) */ /* PWM stage scalars */
const int16_t *shape_y_table; /* cal+0x15E (indirect) */ 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 */ const int16_t *pwm_y_table;
uint16_t pwm_period_max; /* hypothesised */ const int16_t *shape_y_table;
/* PWM duty clamp bounds — RAM[0x6058]/RAM[0x605a] flash mirrors. int16_t closed_loop_gain_const;
* Defaults 0x00CD/0x0F32 match the default family's pwm_min/pwm_max. */
uint16_t pwm_min; /* RAM[0x6058] cache */ uint16_t pwm_period_min;
uint16_t pwm_max; /* RAM[0x605a] cache */ uint16_t pwm_period_max;
uint16_t pwm_min;
uint16_t pwm_max;
}; };
/* ── Submap descriptor ──────────────────────────────────────────────── */ /* ── Submap descriptor ──────────────────────────────────────────────── */
struct pwm_submap_descr { struct pwm_submap_descr {
uint16_t flags; /* +0 */ uint16_t flags;
const int16_t *input_ptr; /* +2 (bound at runtime) */ const int16_t *input_ptr;
uint16_t count; /* +4 */ uint16_t count;
const int16_t *x; /* +6 */ const int16_t *x;
uint16_t input_addr; uint16_t input_addr;
}; };
/** Bind descriptor input_ptr fields to rt inputs. */
static inline void pwm_bind_submap_inputs( static inline void pwm_bind_submap_inputs(
pwm_runtime_t *rt, pwm_runtime_t *rt,
pwm_submap_descr_t *descrs, pwm_submap_descr_t *descrs,
@@ -319,15 +266,8 @@ static inline void pwm_bind_submap_inputs(
* PUBLIC API * 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; 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, void pwm_init(pwm_runtime_t *rt,
const pwm_calibration_t *cal, const pwm_calibration_t *cal,
const pwm_flash_t *flash, const pwm_flash_t *flash,
@@ -335,44 +275,17 @@ void pwm_init(pwm_runtime_t *rt,
void pwm_service(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; } 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, int16_t pwm_interp_lookup(const int16_t *x, const int16_t *y,
uint16_t n, int16_t in); 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 pwm_lut_duty(const pwm_calibration_t *cal,
uint16_t rpm, int16_t fbkw); 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_calibration_t pwm_cal_rom;
extern const pwm_flash_t pwm_flash_rom; extern const pwm_flash_t pwm_flash_rom;
/** Descriptor array indices. */
enum pwm_submap_id { enum pwm_submap_id {
PWM_SUBMAP_SETPOINT_INTERP = 0, PWM_SUBMAP_SETPOINT_INTERP = 0,
PWM_SUBMAP_PWM_A = 1, PWM_SUBMAP_PWM_A = 1,

2
Core/CAN_Libs/can_db.c
View File

@@ -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 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_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};*/ 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 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) #elif defined(T06209) || defined(T06216)

7
Core/CAN_Libs/can_manager.c
View File

@@ -114,7 +114,6 @@ void can_manager_set_startup_reply_msg(const CanMessageDef *reply_msg)
{ {
s_startup_reply_msg = 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) 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) #if defined(T06301)
uint8_t startup_count = 9; uint8_t startup_count = 9;
uint8_t startupiscar = 0; 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_count = 0;
uint8_t startup_sent_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); Timeout_StartIfStopped(20, TIM16->CNT - 500);
//no deberia hacer na esto pq bootea started. //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_sent_count){return;}
if(startup_finished){ if(startup_finished){

61
Core/Inc/fuel_map.h
View File

@@ -1,56 +1,11 @@
/* #ifndef FUEL_MAP_T06215_H
* fuel_map.h #define FUEL_MAP_T06215_H
*
* Created on: Oct 18, 2024
* Author: herli
*/
#ifndef FUEL_MAP_H_
#define FUEL_MAP_H_
#include <id.h>
#include <stdint.h>
//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);
/* 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); float FM_GET_PHIAD(float RPM, float ME, float Temp);
struct AlphaStruct { #endif /* FUEL_MAP_T06215_H */
//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_ */

21
Core/Inc/id.h
View File

@@ -12,13 +12,13 @@
/* DEBUG PARAMETERS*/ /* DEBUG PARAMETERS*/
//#define T06301 //ford 004 -> 002 004 006 || 504 -> 010 018 //#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 T15021 //audi 506 -> 030 033
//#define T31804 //audi 506 -> 037 038 //#define T31804 //audi 506 -> 037 038
//#define T06209 //eq: T06216 //#define T06209 //eq: T06216
#define T06211 //#define T06211
#define _504012 #define _424026
/* FORD */ /* FORD */
#define FORD_SYNC_PULSE_OUT 0 #define FORD_SYNC_PULSE_OUT 0
@@ -29,22 +29,19 @@
#define CYLINDERS 4 #define CYLINDERS 4
/* TIMING COMPENSATIONS */ /* 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 //(phiad - injangle with fault tein)*totime = teinnom
#define TEIN_FAULT 950 #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_MIN -5.367
#define FBKW_FEEDBACK_MAX 21.27 #define FBKW_FEEDBACK_MAX 21.27
#define FBKW_FEEDBACK_IC_DT 27 #define FBKW_FEEDBACK_IC_DT 18
#define FBKW_MAX 90 //en 504 parece que era 506
#define FBKW_MAX_REAL_DEM 165 //wtf is this
/* CAN DEFINITIONS */ /* CAN DEFINITIONS */
#define CAN_BAUDRATE 500 #define CAN_BAUDRATE 500

91
Core/Inc/id_424026.h Normal file
View File

@@ -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_ */

6
Core/Kline_Libs/psg_prop.h
View File

@@ -52,6 +52,12 @@
#define PSG_KUNDENNUMMER1_STR "167005M321" #define PSG_KUNDENNUMMER1_STR "167005M321"
#define PSG_KUNDENNUMMER2_STR " " #define PSG_KUNDENNUMMER2_STR " "
#define PSG_MOD_INDEX_STR "000001" #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 #else
/* Modification index — 6 ASCII chars, ROM 0x93A7 */ /* Modification index — 6 ASCII chars, ROM 0x93A7 */

142
Core/Phi/compute_temp_phi_comp.c Normal file
View File

@@ -0,0 +1,142 @@
#include "compute_temp_phi_comp.h"
#include <stdint.h>
/*
* 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;
}

51
Core/Phi/compute_temp_phi_comp.h Normal file
View File

@@ -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 */

1210
Core/Phi/phi.c Normal file
View File

File diff suppressed because it is too large Load Diff

263
Core/Phi/phi.h Normal file
View File

@@ -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 <stdint.h>
#include <stddef.h>
/* ─── 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 */

247
Core/Phi/phi_cal_tables.c Normal file
View File

@@ -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 <stddef.h>
#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;
}

967
Core/Src/fuel_map.c
View File

@@ -1,885 +1,112 @@
/* /*
* fuel_map.c * fuel_map.c — T06215 variant
* *
* Created on: Oct 18, 2024 * Thin wrapper that exposes the legacy host-app API
* Author: herli * (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 <id.h>
#include "fuel_map.h" #include "fuel_map.h"
#include "phi.h"
#include <stdint.h>
#include <stddef.h>
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) { /* ──── Getters (nullary; T06215 vtable shape) ────────────────────────── */
//fuelmap = fuelmap_25;
//fuelmap2 = fuelmap_74; static uint16_t get_rpm(void) { return (uint16_t)(s_rpm * CF_TMS); }
indexes = fuelMapI; static int16_t get_inj_qty_demand(void) { return (int16_t) (s_me * CF_ME); }
phi_pointer = PHIAD; 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){ void Timer1_FM_ISR(){
float Z = 0; phi_tick_1khz(&s_phi_state, &s_phi_cal);
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]
} }
float weirdasscorrection; extern float forceTemp;
float GetAlpha(float RPM, float ME, float Tein, float Temp){ float FM_GET_PHIAD(float RPM, float ME, float Temp) {
int RPMBounds = -1; // (-1)-Central Point, 0-Outside Minimum, 1-Outside Maximum s_rpm = RPM;
int MEBounds = -1; s_me = ME;
s_temp = Temp;
int I_RPM = 0; phi_service(&s_phi_state, &s_phi_cal, &s_phi_out);
int I_ME = 0;
float Z = 0; float Z = (float)s_phi_out.angle_accumulator * ANGLE_DEG_PER_RAW;
if(ME < 0.031){//0.031 if (s_phiad_out) *s_phiad_out = Z;
*phi_pointer = 0.0; return Z;
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 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

6
Core/Src/injection.c
View File

@@ -98,7 +98,7 @@ 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, forceTemp); FM_GET_PHIAD(MT_RPM, ME, Temp);
float target = INJ_UPDATE_TARGET_EOI() + correction_eoi; 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 += TIMETODEG(INJ_CLOSING_MARGIN, TEETH_RPM);
} }
current_Alpha = current_Alpha < 0.1 ? 0 : current_Alpha; current_Alpha = current_Alpha < 0.1 ? 0 : current_Alpha;
boostmult = BoostMultiplier(inj_mode, RPM, ME); //boostmult = BoostMultiplier(inj_mode, RPM, ME);
current_Alpha *= boostmult; //current_Alpha *= boostmult;
} }

8
Core/Src/main.c
View File

@@ -188,7 +188,11 @@ uint32_t lastPID_t = 0;
//extern uint8_t forceDC2; //extern uint8_t forceDC2;
void OnEnd(){ void OnEnd(){
safetySHUTOFF = 1; safetySHUTOFF = 1;
#if defined(T06301)
startup = 2; startup = 2;
#endif
startedEngine = 0; startedEngine = 0;
//forceDC2 = 2; //forceDC2 = 2;
@@ -268,7 +272,7 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
if (htim->Instance == TIM17) KL_Session_OnTim17Elapsed(); if (htim->Instance == TIM17) KL_Session_OnTim17Elapsed();
} }//
/* USER CODE END 0 */ /* USER CODE END 0 */
/** /**
@@ -1132,7 +1136,7 @@ static void MX_TIM6_Init(void)
htim6.Instance = TIM6; htim6.Instance = TIM6;
htim6.Init.Prescaler = 160-1; htim6.Init.Prescaler = 160-1;
htim6.Init.CounterMode = TIM_COUNTERMODE_UP; htim6.Init.CounterMode = TIM_COUNTERMODE_UP;
htim6.Init.Period = 11099; htim6.Init.Period = 9999;
htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim6) != HAL_OK) if (HAL_TIM_Base_Init(&htim6) != HAL_OK)
{ {

1
Core/Src/stm32g4xx_it.c
View File

@@ -401,6 +401,7 @@ void TIM6_DAC_IRQHandler(void)
/* USER CODE BEGIN TIM6_DAC_IRQn 1 */ /* USER CODE BEGIN TIM6_DAC_IRQn 1 */
//FBKW_PIDInterrupt(); //FBKW_PIDInterrupt();
FBKW_service(); FBKW_service();
Timer1_FM_ISR();
/* USER CODE END TIM6_DAC_IRQn 1 */ /* USER CODE END TIM6_DAC_IRQn 1 */
} }

9
Core/Src/toothed_wheel.c
View File

@@ -73,7 +73,7 @@ void TW_EVAL_MAX_TEETH(){
TW_RPM_SENSOR_STATE = (TW_current_max_teeth == TW_PERCYL_TEETH) ? 1 : 0 ; TW_RPM_SENSOR_STATE = (TW_current_max_teeth == TW_PERCYL_TEETH) ? 1 : 0 ;
} }
uint32_t sensorfail; uint32_t sensorfail;
extern uint8_t startup_finished;
volatile uint8_t isMT = 0; volatile uint8_t isMT = 0;
uint8_t SYNCOUT_TEETH = 0; uint8_t SYNCOUT_TEETH = 0;
volatile float accel_rpm = 0; volatile float accel_rpm = 0;
@@ -137,7 +137,7 @@ void TW_TEETH_CAPTURE(){
TW_PREPARE_BOI_ACCELCORRECTION(); 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) can_port_send_msg_def(&MSG_ID_EMPF3); // TX from template (+ symbols if any)
Timeout_ResetByIndex(18, TIM16->CNT); // re-arm for another 40ms 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); //meter timeout cuando !count_ckp (significa que solo recibe un pulso);
CKP_PULSE_AVAILABLE = 1; CKP_PULSE_AVAILABLE = 1;
Timeout_ResetByIndex(13, TIM16->CNT); // Reset CKP timeout Timeout_ResetByIndex(13, TIM16->CNT); // Reset CKP timeout
FBKW_CKP_ISR();
} }
} }
float fb_1; float fb_1;
@@ -357,6 +356,7 @@ void TW_CALC_FBKW_FEEDBACK(){
difftop = fb_2 - FBKW_FEEDBACK_MAX; difftop = fb_2 - FBKW_FEEDBACK_MAX;
valid = 1; valid = 1;
} }
}else{ }else{
//feedbackfirst = RPM * 6.0 * diffCKP / refClock; //feedbackfirst = RPM * 6.0 * diffCKP / refClock;
if(feedbackMT){ if(feedbackMT){
@@ -376,7 +376,10 @@ void TW_CALC_FBKW_FEEDBACK(){
diffbot = FBKW_FEEDBACK_MIN - fb_1; diffbot = FBKW_FEEDBACK_MIN - fb_1;
difftop = fb_1 - FBKW_FEEDBACK_MAX; 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 if(!valid && count_CKP > 1){ //si no encontro ningun otro nuevo valor dentro de rango, checkear boundaries

2
hpsg5-controller_v2-stm32g4.ioc
View File

@@ -373,7 +373,7 @@ TIM4.PeriodNoDither=29851
TIM4.Prescaler=80-1 TIM4.Prescaler=80-1
TIM4.PulseNoDither_2=5000 TIM4.PulseNoDither_2=5000
TIM6.IPParameters=Prescaler,PeriodNoDither TIM6.IPParameters=Prescaler,PeriodNoDither
TIM6.PeriodNoDither=11100-1 TIM6.PeriodNoDither=10000-1
TIM6.Prescaler=160-1 TIM6.Prescaler=160-1
TIM7.IPParameters=Prescaler,PeriodNoDither,TIM_MasterOutputTrigger TIM7.IPParameters=Prescaler,PeriodNoDither,TIM_MasterOutputTrigger
TIM7.PeriodNoDither=9 TIM7.PeriodNoDither=9