diff --git a/.cproject b/.cproject
index b2a5138..bad336f 100644
--- a/.cproject
+++ b/.cproject
@@ -25,7 +25,7 @@
-
+
@@ -85,7 +85,7 @@
-
+
@@ -171,7 +171,7 @@
-
+
diff --git a/Core/Advance_Control/FBKW.c b/Core/Advance_Control/FBKW.c
index d7c03fc..88b8a09 100644
--- a/Core/Advance_Control/FBKW.c
+++ b/Core/Advance_Control/FBKW.c
@@ -25,18 +25,6 @@ extern float B_PHIAD;
uint8_t CKP_PULSE_AVAILABLE = 0;
uint8_t FBKW_PID_OPEN = 0; //estaba en 0 antes, igual asi no se peta el pid si no recibe avance
-void UpdateFBKW_MODULATION(TIM_HandleTypeDef* pwmHandle, uint32_t pwmChannel, float dutyCycle){
- //dutyCycle = F_clamp(dutyCycle, 5.0, 95.0);
- uint32_t newRegVal = (uint32_t)roundf((float)(pwmHandle->Instance->ARR) * (dutyCycle * 0.01f));
-
- /*In case of the dutyCycle being calculated as higher than the reload register, cap it to the reload register*/
- if(newRegVal > pwmHandle->Instance->ARR)
- {
- newRegVal = pwmHandle->Instance->ARR;
- }
- /*Assign the new dutyCycle count to the capture compare register.*/
- __HAL_TIM_SET_COMPARE(pwmHandle, pwmChannel, (uint32_t)(roundf(newRegVal)));
-}
void UpdatePWM(TIM_HandleTypeDef* pwmHandle, uint32_t pwmChannel, uint16_t period_on, uint16_t period){
if(period_on > period)
@@ -147,32 +135,42 @@ void FBKW_init(void) {
pwm_init(&fbkw_rt, &pwm_cal_rom, &pwm_flash_rom, &fbkw_getters);
}
+uint8_t last_ckp = 1;
void FBKW_service(void) {
/* Signal a reset edge to the supervisor when CKP pulses are not yet
* available — matches the VP44 boot/CKP-lost reset behavior. */
if (!CKP_PULSE_AVAILABLE) {
+ if(last_ckp){
+ //fbkw_rt.system_flags_110 = 0x30;
+ fbkw_rt.system_flags_110 |= 0x20u; // set ONLY bit 5 (OL gate). Leave other bits alone.
+
+
+#if defined(T06211)
+#elif defined(T06301)
+ fbkw_rt.mode_flags = 1;//muy importantes estas mierdas para que en open loop vaya como toca.
+#endif
+ }
fbkw_rt.reset_flag = 1;
- fbkw_rt.system_flags_110 = 0x30;
- fbkw_rt.mode_flags = 1;//muy importantes estas mierdas para que en open loop vaya como toca.
+ }else{
+ if(!last_ckp){
+ fbkw_rt.system_flags_110 &= ~0x20u; // clear ONLY bit 5 (OL gate). Leave other bits alone.
+
+#if defined(T06211)
+ fbkw_rt.pi_open_loop_flag = 0x00; //0x2000
+#endif
+ }
}
+ last_ckp = CKP_PULSE_AVAILABLE;
+
pwm_service(&fbkw_rt);
- /* pwm_duty is a 12-bit (0..4095) command. Convert to percentage for
- * the existing UpdateFBKW_MODULATION helper, which expects 0..100. */
- /*float duty_pct = ((float)fbkw_rt.pwm_duty) * (100.0f / 4095.0f);
- duty_pct = duty_pct < FBKW_PWM_MIN ? FBKW_PWM_MIN : duty_pct;
- duty_pct = duty_pct > FBKW_PWM_MAX ? FBKW_PWM_MAX : duty_pct;
-
- if (forceDC) {
- FBKW_DC = (float)forceDC;
- } else {
- FBKW_DC = duty_pct;
- }*/
UpdatePWM(&htim4, TIM_CHANNEL_2, fbkw_rt.pwm_on_time, fbkw_rt.pwm_period);
FBKW_DC = ((float)fbkw_rt.pwm_duty) * (100.0f / 4095.0f);
- FBKW_DEMAND = fbkw_rt.target * (3.0f / 256.0f);
- //UpdateFBKW_MODULATION(&htim4, TIM_CHANNEL_2, FBKW_DC);
+ FBKW_DEMAND = fbkw_rt.target_5e * (3.0f / 256.0f);
+}
+void FBKW_CKP_ISR(void) {
+ pwm_ckp_isr(&fbkw_rt);
}
const pwm_runtime_t *FBKW_pipeline_runtime(void) {
diff --git a/Core/Advance_Control/FBKW.h b/Core/Advance_Control/FBKW.h
index 12cb08a..f830dca 100644
--- a/Core/Advance_Control/FBKW.h
+++ b/Core/Advance_Control/FBKW.h
@@ -20,13 +20,6 @@ extern volatile float RPM;
extern float refClock;
extern uint8_t safetySHUTOFF;
-
-extern float errorFBKW;
-extern float targetFBKW;
-extern float Kp;
-extern float Ki;
-extern float intFBKWerror; //add += error * tstep
-
extern float FBKW_DEMAND;
extern float FBKW_DC;
extern float FBKW_FEEDBACK;
@@ -36,40 +29,12 @@ extern int16_t B_FB_NW;
extern uint8_t forceDC;
-typedef struct PID
-{
- float Kp; // Proportional gain constant
- float Ki; // Integral gain constant
- float Kd; // Derivative gain constant
- float Kaw; // Anti-windup gain constant
- float Bias; // Output bias
- float T_C; // Time constant for derivative filtering
- float T; // Time step
- float max; // Max command
- float min; // Min command
- float max_rate; // Max rate of change of the command
- float integral; // Integral term
- float err_prev; // Previous error
- float deriv_prev; // Previous derivative
- float command_sat_prev;// Previous saturated command
- float command_prev; // Previous command
-} PID;
-extern PID myPID; //antes era volatile
-extern void FBKW_RESET_CKP_COUNT();
+/*extern void FBKW_RESET_CKP_COUNT();
extern void FBKW_PIDInterrupt();
extern void FBKW_PROCESS_CKP_PULSE();
-extern void updatePIDfreq(struct PID *pid, uint8_t millis);
-
-extern void definePID(struct PID *pid, float Kp, float Ki, float Kd, float Kaw, float Bias, float T_C, float T, float max, float min, float max_rate, float integral, float err_prev, float deriv_prev, float command_sat_prev, float command_prev);
-extern void initPID(struct PID *pid, float T_C, float T);
-
-extern void UpdateFBKW_MODULATION(TIM_HandleTypeDef* pwmHandle, uint32_t pwmChannel, float dutyCycle);
-extern float UpdateFBKW_DEMAND(float FBKW);
-float UpdateFBKW_OpenDutyCycle(float RPM);
-extern void UpdatePID(struct PID *pid);
-
-float F_clamp(float val, float min, float max);
+extern void updatePIDfreq(struct PID *pid, uint8_t millis);*/
+extern void FBKW_CKP_ISR();
/* ---- VP44 reverse-engineered pipeline bridge ---------------------------
* FBKW_init : call once at startup (after RPM/Temp/etc globals exist).
diff --git a/Core/Advance_Control/cal_tables_rom.c b/Core/Advance_Control/cal_tables_rom.c
index 7ab9c35..7732253 100644
--- a/Core/Advance_Control/cal_tables_rom.c
+++ b/Core/Advance_Control/cal_tables_rom.c
@@ -1,156 +1,138 @@
/**
- * @file cal_tables_rom.c
- * @brief ROM-extracted calibration data for the VP44 PWM controller (compact).
+ * @file cal_tables_rom.c (families/t06211/compact_src)
+ * @brief ROM-decoded t06211 calibration.
*
* AUTO-GENERATED by tools/extract_calibration.py
- * Source ROM: rom_eeprom_dump_0000-9FFF_004002.bin
- * Calibration base (RWA4): 0x9C28
- * Flash anchor: 0x9618
- * Generated: 2026-04-20 13:49:45
+ * Source ROM: rom_eeprom_dump_0000-9FFF_504012.bin
+ * Calibration base (RWA4): 0x9BD8
+ * Flash anchor: 0x7E18
+ * Generated: 2026-04-27 12:40:48
*
* DO NOT EDIT — regenerate with:
- * python tools/extract_calibration.py rom_eeprom_dump_0000-9FFF_004002.bin --target compact
+ * python tools/extract_calibration.py --family t06211
*/
-#include "cal_tables_rom.h"
+#include "pwm.h"
-/* == Submap table data ==
- *
- * Per-submap x-arrays (consumed by s_eval via descriptor.x), Y-table
- * payloads used by s_combine (2D) and s_refine (1D), and the runtime
- * descriptor array. Definitions come first so the pwm_cal_rom /
- * pwm_flash_rom literals below can reference the Y-table statics by name.
- */
+/* ── Submap x/y arrays ──────────────────────────────────────────────── */
-/* max_error descriptor at CAL+0x0136:
- * flags=0x0000, input_var=0x0040 (rpm), count=4, x_ptr=0x9DFC */
-static const int16_t submap_max_error_x[] = {10066, 3565, 1678, 0};
-static const int16_t submap_max_error_y[] = {1536, 1365, 853, 85};
+static const int16_t setpoint_x[6] = { 8389, 5872, 4614, 2726, 1426, 0 };
+static const int16_t setpoint_y[6] = { 1707, 1707, 1707, 939, 427, 427 };
-/* pwm_A descriptor at CAL+0x0140:
- * flags=0x0000, input_var=0x0040 (rpm), count=6, x_ptr=0x9E36 */
-static const int16_t submap_pwm_A_x[] = {20972, 15099, 10066, 3565, 1678, 0};
-static const int16_t submap_pwm_A_y[] = {1536, 1365, 1195, 853, 427, 85};
+static const int16_t pwm_A_x[9] = { 25166, 18455, 13841, 8389, 5872, 4614, 2726, 1426, 0 };
+static const int16_t pwm_A_y[9] = { 1707, 1365, 1195, 768, 427, 85, 0, -171, -469 };
-/* pwm_B descriptor at CAL+0x0148:
- * flags=0x0000, input_var=0x0046 (some form of target/demand angle), count=7, x_ptr=0x9E42 */
-static const int16_t submap_pwm_B_x[] = {1536, 1365, 1195, 853, 427, 85, 0};
-static const int16_t submap_pwm_B_y[] = {819, 737, 492, 0, 41, 49, 82};
+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 };
-/* shape_eval descriptor at CAL+0x0152:
- * flags=0x0000, input_var=0x0142 (voltage), count=4, x_ptr=0x9E50 */
-static const int16_t submap_shape_eval_x[] = {819, 737, 492, 0};
-static const int16_t submap_shape_eval_y[] = {41, 49, 82, 102};
+static const int16_t shape_x[4] = { 819, 737, 492, 0 };
+static const int16_t shape_y[4] = { 41, 49, 82, 102 };
-/* sp0_A descriptor at CAL+0x015C:
- * flags=0x0000, input_var=0x0040 (rpm), count=9, x_ptr=0x9E0C */
-static const int16_t submap_sp0_A_x[] = {20972, 16777, 14680, 12583, 10486, 8389, 6291, 4194, 0};
-static const int16_t submap_sp0_A_y[] = {12583, 0, 960, 640, 320, 0, 5968, 5088, 4928};
+/* ── Y-tables (dereferenced from pwm_y_table_ptr / shape_y_table_ptr) ── */
-/* sp0_B descriptor at CAL+0x0164:
- * flags=0x0000, input_var=0x0044 (inj qty. demand), count=4, x_ptr=0x9E22 */
-static const int16_t submap_sp0_B_x[] = {960, 640, 320, 0};
-static const int16_t submap_sp0_B_y[] = {5968, 5088, 4928, 0};
+static const int16_t shape_y_table_rom[4] = { 41, 49, 82, 102 };
-/* sp1_B descriptor at CAL+0x016C:
- * flags=0x0000, input_var=0x0146 (some form of temperature), count=4, x_ptr=0x9E2A */
-static const int16_t submap_sp1_B_x[] = {5968, 5088, 4928, 0};
-static const int16_t submap_sp1_B_y[] = {256, 0, 20972, 15099};
-
-/* sp2_B descriptor at CAL+0x0174:
- * flags=0x0000, input_var=0x0042 (angle inj qty. decrease comman), count=2, x_ptr=0x9E32 */
-static const int16_t submap_sp2_B_x[] = {256, 0};
-static const int16_t submap_sp2_B_y[] = {20972, 15099};
-
-/* sp2_A descriptor at CAL+0x017C:
- * flags=0x0000, input_var=0x0040 (rpm), count=2, x_ptr=0x9E1E */
-static const int16_t submap_sp2_A_x[] = {12583, 0};
-static const int16_t submap_sp2_A_y[] = {960, 640};
-
-/* y_pair_0 @ 0x9EB4: 9x4 = 36 int16 words */
-static const int16_t pwm_y_y_pair_0_data[] = {2912, 2526, 2333, 2206, 2067, 1878, 1708, 1539, 1374, 2696, 2442, 2315, 2185, 2068, 1891, 1736, 1555, 1407, 2655, 2383, 2247, 2111, 1981, 1845, 1725, 1497, 1349, 2526, 2312, 2204, 2064, 1939, 1828, 1733, 1475, 1336};
-
-/* y_pair_1 @ 0x9EFC: 9x4 = 36 int16 words */
-static const int16_t pwm_y_y_pair_1_data[] = {166, 93, 50, 11, 22, (int16_t)0xFFF2, (int16_t)0xFFBF, (int16_t)0xFF9E, (int16_t)0xFF5F, 13, 7, 4, 1, 2, (int16_t)0xFFFF, (int16_t)0xFFFB, (int16_t)0xFFF8, (int16_t)0xFFE7, 0, 0, 0, 0, 0, 0, 0, 0, 0, (int16_t)0xFCEC, (int16_t)0xFE46, (int16_t)0xFF12, (int16_t)0xFFCA, (int16_t)0xFF98, 66, 308, 467, 762};
-
-/* y_pair_2 @ 0x9F44: 2x2 = 4 int16 words */
-static const int16_t pwm_y_y_pair_2_data[] = {0, 0, 0, 0};
-
-/* pwm_y_table @ 0x9E60: 6x7 = 42 int16 words */
-static const int16_t pwm_y_pwm_y_table_data[] = {205, 205, 205, 205, 205, 205, 880, 799, 573, 512, 205, 205, 1106, 983, 758, 594, 205, 205, 1536, 1392, 1085, 778, 287, 205, 2559, 2150, 1740, 1392, 594, 205, 3481, 3174, 2518, 2252, 983, 205, 3890, 3890, 3890, 3890, 3890, 3890};
-
-/* shape_y_table @ 0x9E58: 4x1 = 4 int16 words */
-static const int16_t pwm_y_shape_y_table_data[] = {41, 49, 82, 102};
-
-/* Runtime-mutable descriptor array. `input_ptr` is resolved at
- * boot by pwm_bind_submap_inputs(&rt, pwm_submap_descrs,
- * PWM_SUBMAP_COUNT). The enum is declared in cal_tables_rom.h. */
-pwm_submap_descr_t pwm_submap_descrs[PWM_SUBMAP_COUNT] = {
- [PWM_SUBMAP_PWM_A] = { .flags=0x0000, .input_ptr=NULL, .count=6, .x=submap_pwm_A_x, .input_addr=0x0040 },
- [PWM_SUBMAP_PWM_B] = { .flags=0x0000, .input_ptr=NULL, .count=7, .x=submap_pwm_B_x, .input_addr=0x0046 },
- [PWM_SUBMAP_SHAPE_EVAL] = { .flags=0x0000, .input_ptr=NULL, .count=4, .x=submap_shape_eval_x, .input_addr=0x0142 },
- [PWM_SUBMAP_SP0_A] = { .flags=0x0000, .input_ptr=NULL, .count=9, .x=submap_sp0_A_x, .input_addr=0x0040 },
- [PWM_SUBMAP_SP0_B] = { .flags=0x0000, .input_ptr=NULL, .count=4, .x=submap_sp0_B_x, .input_addr=0x0044 },
- [PWM_SUBMAP_SP1_B] = { .flags=0x0000, .input_ptr=NULL, .count=4, .x=submap_sp1_B_x, .input_addr=0x0146 },
- [PWM_SUBMAP_SP2_B] = { .flags=0x0000, .input_ptr=NULL, .count=2, .x=submap_sp2_B_x, .input_addr=0x0042 },
- [PWM_SUBMAP_SP2_A] = { .flags=0x0000, .input_ptr=NULL, .count=2, .x=submap_sp2_A_x, .input_addr=0x0040 },
+static const int16_t pwm_y_table_rom[90] = {
+ /* row 0 */ 205, 205, 205, 205, 205, 205, 0, 0, 0,
+ /* row 1 */ 1638, 1392, 1229, 962, 778, 512, 0, 0, 0,
+ /* row 2 */ 1843, 1577, 1433, 1208, 1003, 758, 0, 0, 0,
+ /* row 3 */ 2457, 2109, 1986, 1761, 1577, 1229, 287, 0, 0,
+ /* row 4 */ 2867, 2744, 2641, 2457, 2314, 2129, 1269, 205, 205,
+ /* row 5 */ 3686, 3481, 3460, 3378, 3276, 3174, 2907, 1024, 737,
+ /* row 6 */ 3890, 3870, 3849, 3829, 3808, 3788, 3235, 1310, 1024,
+ /* row 7 */ 3890, 3890, 3890, 3890, 3890, 3890, 3890, 2076, 1638,
+ /* row 8 */ 3890, 3890, 3890, 3890, 3890, 3890, 3890, 3890, 3481,
+ /* row 9 */ 3890, 3890, 3890, 3890, 3890, 3890, 3890, 3890, 3890,
};
-/* ── ROM-extracted calibration (TABLE[RWA4]+offset) ─────────────────── */
+/* ── 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,
+ },
+ [PWM_SUBMAP_PWM_A] = {
+ .flags = 0, .input_ptr = NULL, .count = 9,
+ .x = pwm_A_x, .input_addr = 0x0040,
+ },
+ [PWM_SUBMAP_PWM_B] = {
+ .flags = 0, .input_ptr = NULL, .count = 10,
+ .x = pwm_B_x, .input_addr = 0x0046,
+ },
+ [PWM_SUBMAP_SHAPE_EVAL] = {
+ .flags = 0, .input_ptr = NULL, .count = 4,
+ .x = shape_x, .input_addr = 0x0142,
+ },
+};
+
+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 ────────────────────────────────────────────────────────── */
const pwm_calibration_t pwm_cal_rom = {
- .target_minimum = 0, /* CAL+0x011E */
- .y_pair = {
- pwm_y_y_pair_0_data, /* CAL+0x0184 @ 0x9EB4 */
- pwm_y_y_pair_1_data, /* CAL+0x0186 @ 0x9EFC */
- pwm_y_y_pair_2_data, /* CAL+0x0188 @ 0x9F44 */
- },
+ .large_pos_error_thresh = 128, /* cal+0x10E */
+ .large_neg_error_thresh = (int16_t)0xFF00, /* cal+0x110 */
+ .pi_low_clamp = (int16_t)0xFE00, /* cal+0x120 */
+ .pi_high_clamp = 1707, /* cal+0x124 */
- /* PI shaping parameters */
- .pi_upper_breakpoint = 853, /* CAL+0x00FE */
- .pi_lower_breakpoint = (int16_t)0xFCAB, /* CAL+0x0100 */
- .pi_center_slope = 2560, /* CAL+0x010C */
- .pi_upper_outer_slope = 2560, /* CAL+0x010E */
- .pi_lower_outer_slope = 2560, /* CAL+0x0110 */
- .pi_upper_integrator_gain = 1024, /* CAL+0x0112 */
- .pi_center_integrator_gain = 1024, /* CAL+0x0114 */
- .pi_lower_integrator_gain = 1024, /* CAL+0x0116 */
+ /* CAN-decoded setpoint (FUN_64c3) cal constants */
+ .b_fb_kw_upper_bound = 7680, /* cal+0x004 */
+ .b_fb_kw_lower_bound = (int16_t)0xFD00, /* cal+0x006 */
+ /* setpoint_offset = cal+0x4c - cal+0x4e = 3499 - 4156 = -657 */
+ .setpoint_offset = (int16_t)0xFD6F,
+ .target_5e_min_clamp = (int16_t)0xFE00, /* cal+0x122 */
+ .can_aux_12e_max = 1451, /* cal+0x002 */
+ .error_thresh_114 = 100, /* cal+0x114 */
+ .pi_thresh_116 = 96, /* cal+0x116 */
+ .pi_sat_count_threshold = 800, /* cal+0x112 = 0x0320 */
+ .rpm_threshold_11E = 2936, /* cal+0x11E */
+ .pi_cl_rpm_floor = 420, /* RAM[0x605c] = 0x01A4 */
- /* compute_closed_loop_correction / fast_recovery tuning */
- .closed_loop_gain_const = 4, /* CAL+0x0156 */
- .error_persist_init_count = 100, /* CAL+0x0108 */
- .recovery_rpm_threshold = 4194, /* CAL+0x011A */
+ .pwm_detail_x0 = (int16_t)0x9C40, /* cal+0x0EE */
+ .pwm_detail_x1 = (int16_t)0x8235, /* cal+0x0F0 */
+ .pwm_cached_ptr_0F2 = 5662, /* cal+0x0F2 (legacy scalar) */
+ .pwm_cached_ptr_102 = 168, /* cal+0x102 (legacy scalar) */
- /* Setpoint offset latched at init by FUN_8643 (0x867d):
- * CAL[0x52]=2167 - CAL[0x54]=2833 = -666
- * (RAM[0x0430] always 0, so omitted). */
- .setpoint_offset = (int16_t)0xFD66, /* CAL+0x0052 - CAL+0x0054 */
+ /* RPM-window matching: inline 8-int16 band array at cal+0x0F2..0x100
+ * (4 (lo,hi) pairs) plus halfwidth at cal+0x102. Drives the three-
+ * phase pwm_period slew in FUN_5314 (open-questions §5 closeout). */
+ .pwm_rpm_windows = { 5662, 6795, 8808, 10486, 11954, 13422, 18036, 19713 },
+ .pwm_window_halfwidth = 168,
+
+ /* pwm_const_104 / pwm_slew_step are the same value (cal+0x104 = 354);
+ * pwm_const_104 retained for backward compat, pwm_slew_step is the
+ * semantic name used by the RPM-window matcher (per-cycle pwm_period
+ * slew magnitude, applied at Phase 1 / Phase 3). Cached to
+ * RAM[0x02ec] at FUN_5314:0x53b9. */
+ .pwm_const_104 = 354, /* cal+0x104 (legacy alias) */
+ .pwm_slew_step = 354, /* cal+0x104 (semantic) */
+
+ .pwm_y_table = pwm_y_table_rom, /* cal+0x154 @ 0x9E28 */
+ .shape_y_table = shape_y_table_rom, /* cal+0x15E @ 0x9E20 */
+
+ /* CL-correction gain — cached at absolute ROM[0x6056], not cal-relative. */
+ .closed_loop_gain_const = 10, /* ROM[0x6056] */
+
+ /* PWM period endpoints (alias of pwm_detail_x0/x1) as unsigned. */
+ .pwm_period_min = 33333, /* cal+0x0F0 */
+ .pwm_period_max = 40000, /* cal+0x0EE */
+
+ /* Duty clamp bounds — RAM[0x6058]/RAM[0x605a] mirrors. Same defaults
+ * as the default family's pwm_min/pwm_max. */
+ .pwm_min = 0x00CD, /* RAM[0x6058] = 205 */
+ .pwm_max = 0x0F32, /* RAM[0x605a] = 3890 */
};
-/* ── ROM-extracted flash tables (rebased from FLASH:xxxx) ───────────── */
-
-const pwm_flash_t pwm_flash_rom = {
- /* Max-error interpolation table (descriptor at CAL+0x0136)
- * Input variable: 0x0040 (rpm)
- * 4 entries, x-array descending */
- .max_error_x = {10066, 3565, 1678, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
- .max_error_y = {1536, 1365, 853, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
- .max_error_count = 4,
- .max_error_input_addr = 0x0040,
-
- /* PI controller thresholds */
- .request_upper_limit = 0, /* CAL+0x011C (FLASH:9734) */
- .large_pos_error_thresh = 171, /* CAL+0x0102 (FLASH:971A) */
- .large_neg_error_thresh = (int16_t)0xFF55, /* CAL+0x0104 (FLASH:971C) */
- .inj_qty_demand_thresh = 160, /* CAL+0x010A (FLASH:9722) */
-
- /* PWM shaping tables */
- .rpm_breakpoints = {3775, 4614, 7969, 8892, 12163, 13086, 16358, 17281},
- .rpm_values = {168, 354, 650, 500, 853, 64683, 171, 65365},
- .shape_scale_src = 354, /* CAL+0x00F8 (FLASH:9710) */
- .period_max_limit = 29851, /* CAL+0x00E2 (FLASH:96FA) */
- .period_min_limit = 23529, /* CAL+0x00E4 (FLASH:96FC) */
-
- /* PWM submap Y-tables (pointers into ROM) */
- .pwm_y_table = pwm_y_pwm_y_table_data, /* CAL+0x0150 @ 0x9E60 */
- .shape_y_table = pwm_y_shape_y_table_data, /* CAL+0x015A @ 0x9E58 */
-};
+/* Family-1 API parity placeholder — t06211 keeps Y-tables inside
+ * pwm_cal_rom, so pwm_flash_rom has no data. Callers pass &pwm_flash_rom
+ * to pwm_init() purely for signature compatibility. */
+const pwm_flash_t pwm_flash_rom = { 0 };
diff --git a/Core/Advance_Control/cal_tables_rom.h b/Core/Advance_Control/cal_tables_rom.h
index c3bf61a..54682cc 100644
--- a/Core/Advance_Control/cal_tables_rom.h
+++ b/Core/Advance_Control/cal_tables_rom.h
@@ -1,39 +1,12 @@
/**
- * @file cal_tables_rom.h
- * @brief Extern declarations for ROM-extracted calibration data (compact).
- *
- * AUTO-GENERATED by tools/extract_calibration.py
- * Source ROM: rom_eeprom_dump_0000-9FFF_004002.bin
- * Calibration base (RWA4): 0x9C28
- *
- * DO NOT EDIT — regenerate with:
- * python tools/extract_calibration.py rom_eeprom_dump_0000-9FFF_004002.bin --target compact
+ * @file cal_tables_rom.h (families/t06211/compact_src)
+ * @brief Extern decls for the ROM-decoded t06211 calibration.
+ * All declarations live in pwm.h; this file just re-exports the enum
+ * and array symbols for the compact_src/pwm.c translation unit.
*/
-#ifndef CAL_TABLES_ROM_H
-#define CAL_TABLES_ROM_H
+#ifndef CAL_TABLES_ROM_T06211_COMPACT_H
+#define CAL_TABLES_ROM_T06211_COMPACT_H
#include "pwm.h"
-/** ROM-extracted calibration data (pump-family parameters). */
-extern const pwm_calibration_t pwm_cal_rom;
-
-/** ROM-extracted flash table data (lookup tables and thresholds). */
-extern const pwm_flash_t pwm_flash_rom;
-
-/** Stable indices for the runtime-mutable descriptor array. */
-enum pwm_submap_id {
- PWM_SUBMAP_PWM_A,
- PWM_SUBMAP_PWM_B,
- PWM_SUBMAP_SHAPE_EVAL,
- PWM_SUBMAP_SP0_A,
- PWM_SUBMAP_SP0_B,
- PWM_SUBMAP_SP1_B,
- PWM_SUBMAP_SP2_B,
- PWM_SUBMAP_SP2_A,
- PWM_SUBMAP_COUNT
-};
-
-/** Runtime-mutable descriptor array — bind with pwm_bind_submap_inputs. */
-extern pwm_submap_descr_t pwm_submap_descrs[PWM_SUBMAP_COUNT];
-
-#endif /* CAL_TABLES_ROM_H */
+#endif /* CAL_TABLES_ROM_T06211_COMPACT_H */
diff --git a/Core/Advance_Control/cal_tables_rom_004002.c b/Core/Advance_Control/cal_tables_rom_004002.c
new file mode 100644
index 0000000..7ab9c35
--- /dev/null
+++ b/Core/Advance_Control/cal_tables_rom_004002.c
@@ -0,0 +1,156 @@
+/**
+ * @file cal_tables_rom.c
+ * @brief ROM-extracted calibration data for the VP44 PWM controller (compact).
+ *
+ * AUTO-GENERATED by tools/extract_calibration.py
+ * Source ROM: rom_eeprom_dump_0000-9FFF_004002.bin
+ * Calibration base (RWA4): 0x9C28
+ * Flash anchor: 0x9618
+ * Generated: 2026-04-20 13:49:45
+ *
+ * DO NOT EDIT — regenerate with:
+ * python tools/extract_calibration.py rom_eeprom_dump_0000-9FFF_004002.bin --target compact
+ */
+#include "cal_tables_rom.h"
+
+/* == Submap table data ==
+ *
+ * Per-submap x-arrays (consumed by s_eval via descriptor.x), Y-table
+ * payloads used by s_combine (2D) and s_refine (1D), and the runtime
+ * descriptor array. Definitions come first so the pwm_cal_rom /
+ * pwm_flash_rom literals below can reference the Y-table statics by name.
+ */
+
+/* max_error descriptor at CAL+0x0136:
+ * flags=0x0000, input_var=0x0040 (rpm), count=4, x_ptr=0x9DFC */
+static const int16_t submap_max_error_x[] = {10066, 3565, 1678, 0};
+static const int16_t submap_max_error_y[] = {1536, 1365, 853, 85};
+
+/* pwm_A descriptor at CAL+0x0140:
+ * flags=0x0000, input_var=0x0040 (rpm), count=6, x_ptr=0x9E36 */
+static const int16_t submap_pwm_A_x[] = {20972, 15099, 10066, 3565, 1678, 0};
+static const int16_t submap_pwm_A_y[] = {1536, 1365, 1195, 853, 427, 85};
+
+/* pwm_B descriptor at CAL+0x0148:
+ * flags=0x0000, input_var=0x0046 (some form of target/demand angle), count=7, x_ptr=0x9E42 */
+static const int16_t submap_pwm_B_x[] = {1536, 1365, 1195, 853, 427, 85, 0};
+static const int16_t submap_pwm_B_y[] = {819, 737, 492, 0, 41, 49, 82};
+
+/* shape_eval descriptor at CAL+0x0152:
+ * flags=0x0000, input_var=0x0142 (voltage), count=4, x_ptr=0x9E50 */
+static const int16_t submap_shape_eval_x[] = {819, 737, 492, 0};
+static const int16_t submap_shape_eval_y[] = {41, 49, 82, 102};
+
+/* sp0_A descriptor at CAL+0x015C:
+ * flags=0x0000, input_var=0x0040 (rpm), count=9, x_ptr=0x9E0C */
+static const int16_t submap_sp0_A_x[] = {20972, 16777, 14680, 12583, 10486, 8389, 6291, 4194, 0};
+static const int16_t submap_sp0_A_y[] = {12583, 0, 960, 640, 320, 0, 5968, 5088, 4928};
+
+/* sp0_B descriptor at CAL+0x0164:
+ * flags=0x0000, input_var=0x0044 (inj qty. demand), count=4, x_ptr=0x9E22 */
+static const int16_t submap_sp0_B_x[] = {960, 640, 320, 0};
+static const int16_t submap_sp0_B_y[] = {5968, 5088, 4928, 0};
+
+/* sp1_B descriptor at CAL+0x016C:
+ * flags=0x0000, input_var=0x0146 (some form of temperature), count=4, x_ptr=0x9E2A */
+static const int16_t submap_sp1_B_x[] = {5968, 5088, 4928, 0};
+static const int16_t submap_sp1_B_y[] = {256, 0, 20972, 15099};
+
+/* sp2_B descriptor at CAL+0x0174:
+ * flags=0x0000, input_var=0x0042 (angle inj qty. decrease comman), count=2, x_ptr=0x9E32 */
+static const int16_t submap_sp2_B_x[] = {256, 0};
+static const int16_t submap_sp2_B_y[] = {20972, 15099};
+
+/* sp2_A descriptor at CAL+0x017C:
+ * flags=0x0000, input_var=0x0040 (rpm), count=2, x_ptr=0x9E1E */
+static const int16_t submap_sp2_A_x[] = {12583, 0};
+static const int16_t submap_sp2_A_y[] = {960, 640};
+
+/* y_pair_0 @ 0x9EB4: 9x4 = 36 int16 words */
+static const int16_t pwm_y_y_pair_0_data[] = {2912, 2526, 2333, 2206, 2067, 1878, 1708, 1539, 1374, 2696, 2442, 2315, 2185, 2068, 1891, 1736, 1555, 1407, 2655, 2383, 2247, 2111, 1981, 1845, 1725, 1497, 1349, 2526, 2312, 2204, 2064, 1939, 1828, 1733, 1475, 1336};
+
+/* y_pair_1 @ 0x9EFC: 9x4 = 36 int16 words */
+static const int16_t pwm_y_y_pair_1_data[] = {166, 93, 50, 11, 22, (int16_t)0xFFF2, (int16_t)0xFFBF, (int16_t)0xFF9E, (int16_t)0xFF5F, 13, 7, 4, 1, 2, (int16_t)0xFFFF, (int16_t)0xFFFB, (int16_t)0xFFF8, (int16_t)0xFFE7, 0, 0, 0, 0, 0, 0, 0, 0, 0, (int16_t)0xFCEC, (int16_t)0xFE46, (int16_t)0xFF12, (int16_t)0xFFCA, (int16_t)0xFF98, 66, 308, 467, 762};
+
+/* y_pair_2 @ 0x9F44: 2x2 = 4 int16 words */
+static const int16_t pwm_y_y_pair_2_data[] = {0, 0, 0, 0};
+
+/* pwm_y_table @ 0x9E60: 6x7 = 42 int16 words */
+static const int16_t pwm_y_pwm_y_table_data[] = {205, 205, 205, 205, 205, 205, 880, 799, 573, 512, 205, 205, 1106, 983, 758, 594, 205, 205, 1536, 1392, 1085, 778, 287, 205, 2559, 2150, 1740, 1392, 594, 205, 3481, 3174, 2518, 2252, 983, 205, 3890, 3890, 3890, 3890, 3890, 3890};
+
+/* shape_y_table @ 0x9E58: 4x1 = 4 int16 words */
+static const int16_t pwm_y_shape_y_table_data[] = {41, 49, 82, 102};
+
+/* Runtime-mutable descriptor array. `input_ptr` is resolved at
+ * boot by pwm_bind_submap_inputs(&rt, pwm_submap_descrs,
+ * PWM_SUBMAP_COUNT). The enum is declared in cal_tables_rom.h. */
+pwm_submap_descr_t pwm_submap_descrs[PWM_SUBMAP_COUNT] = {
+ [PWM_SUBMAP_PWM_A] = { .flags=0x0000, .input_ptr=NULL, .count=6, .x=submap_pwm_A_x, .input_addr=0x0040 },
+ [PWM_SUBMAP_PWM_B] = { .flags=0x0000, .input_ptr=NULL, .count=7, .x=submap_pwm_B_x, .input_addr=0x0046 },
+ [PWM_SUBMAP_SHAPE_EVAL] = { .flags=0x0000, .input_ptr=NULL, .count=4, .x=submap_shape_eval_x, .input_addr=0x0142 },
+ [PWM_SUBMAP_SP0_A] = { .flags=0x0000, .input_ptr=NULL, .count=9, .x=submap_sp0_A_x, .input_addr=0x0040 },
+ [PWM_SUBMAP_SP0_B] = { .flags=0x0000, .input_ptr=NULL, .count=4, .x=submap_sp0_B_x, .input_addr=0x0044 },
+ [PWM_SUBMAP_SP1_B] = { .flags=0x0000, .input_ptr=NULL, .count=4, .x=submap_sp1_B_x, .input_addr=0x0146 },
+ [PWM_SUBMAP_SP2_B] = { .flags=0x0000, .input_ptr=NULL, .count=2, .x=submap_sp2_B_x, .input_addr=0x0042 },
+ [PWM_SUBMAP_SP2_A] = { .flags=0x0000, .input_ptr=NULL, .count=2, .x=submap_sp2_A_x, .input_addr=0x0040 },
+};
+
+/* ── ROM-extracted calibration (TABLE[RWA4]+offset) ─────────────────── */
+
+const pwm_calibration_t pwm_cal_rom = {
+ .target_minimum = 0, /* CAL+0x011E */
+ .y_pair = {
+ pwm_y_y_pair_0_data, /* CAL+0x0184 @ 0x9EB4 */
+ pwm_y_y_pair_1_data, /* CAL+0x0186 @ 0x9EFC */
+ pwm_y_y_pair_2_data, /* CAL+0x0188 @ 0x9F44 */
+ },
+
+ /* PI shaping parameters */
+ .pi_upper_breakpoint = 853, /* CAL+0x00FE */
+ .pi_lower_breakpoint = (int16_t)0xFCAB, /* CAL+0x0100 */
+ .pi_center_slope = 2560, /* CAL+0x010C */
+ .pi_upper_outer_slope = 2560, /* CAL+0x010E */
+ .pi_lower_outer_slope = 2560, /* CAL+0x0110 */
+ .pi_upper_integrator_gain = 1024, /* CAL+0x0112 */
+ .pi_center_integrator_gain = 1024, /* CAL+0x0114 */
+ .pi_lower_integrator_gain = 1024, /* CAL+0x0116 */
+
+ /* compute_closed_loop_correction / fast_recovery tuning */
+ .closed_loop_gain_const = 4, /* CAL+0x0156 */
+ .error_persist_init_count = 100, /* CAL+0x0108 */
+ .recovery_rpm_threshold = 4194, /* CAL+0x011A */
+
+ /* Setpoint offset latched at init by FUN_8643 (0x867d):
+ * CAL[0x52]=2167 - CAL[0x54]=2833 = -666
+ * (RAM[0x0430] always 0, so omitted). */
+ .setpoint_offset = (int16_t)0xFD66, /* CAL+0x0052 - CAL+0x0054 */
+};
+
+/* ── ROM-extracted flash tables (rebased from FLASH:xxxx) ───────────── */
+
+const pwm_flash_t pwm_flash_rom = {
+ /* Max-error interpolation table (descriptor at CAL+0x0136)
+ * Input variable: 0x0040 (rpm)
+ * 4 entries, x-array descending */
+ .max_error_x = {10066, 3565, 1678, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+ .max_error_y = {1536, 1365, 853, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+ .max_error_count = 4,
+ .max_error_input_addr = 0x0040,
+
+ /* PI controller thresholds */
+ .request_upper_limit = 0, /* CAL+0x011C (FLASH:9734) */
+ .large_pos_error_thresh = 171, /* CAL+0x0102 (FLASH:971A) */
+ .large_neg_error_thresh = (int16_t)0xFF55, /* CAL+0x0104 (FLASH:971C) */
+ .inj_qty_demand_thresh = 160, /* CAL+0x010A (FLASH:9722) */
+
+ /* PWM shaping tables */
+ .rpm_breakpoints = {3775, 4614, 7969, 8892, 12163, 13086, 16358, 17281},
+ .rpm_values = {168, 354, 650, 500, 853, 64683, 171, 65365},
+ .shape_scale_src = 354, /* CAL+0x00F8 (FLASH:9710) */
+ .period_max_limit = 29851, /* CAL+0x00E2 (FLASH:96FA) */
+ .period_min_limit = 23529, /* CAL+0x00E4 (FLASH:96FC) */
+
+ /* PWM submap Y-tables (pointers into ROM) */
+ .pwm_y_table = pwm_y_pwm_y_table_data, /* CAL+0x0150 @ 0x9E60 */
+ .shape_y_table = pwm_y_shape_y_table_data, /* CAL+0x015A @ 0x9E58 */
+};
diff --git a/Core/Advance_Control/cal_tables_rom_004002.h b/Core/Advance_Control/cal_tables_rom_004002.h
new file mode 100644
index 0000000..c3bf61a
--- /dev/null
+++ b/Core/Advance_Control/cal_tables_rom_004002.h
@@ -0,0 +1,39 @@
+/**
+ * @file cal_tables_rom.h
+ * @brief Extern declarations for ROM-extracted calibration data (compact).
+ *
+ * AUTO-GENERATED by tools/extract_calibration.py
+ * Source ROM: rom_eeprom_dump_0000-9FFF_004002.bin
+ * Calibration base (RWA4): 0x9C28
+ *
+ * DO NOT EDIT — regenerate with:
+ * python tools/extract_calibration.py rom_eeprom_dump_0000-9FFF_004002.bin --target compact
+ */
+#ifndef CAL_TABLES_ROM_H
+#define CAL_TABLES_ROM_H
+
+#include "pwm.h"
+
+/** ROM-extracted calibration data (pump-family parameters). */
+extern const pwm_calibration_t pwm_cal_rom;
+
+/** ROM-extracted flash table data (lookup tables and thresholds). */
+extern const pwm_flash_t pwm_flash_rom;
+
+/** Stable indices for the runtime-mutable descriptor array. */
+enum pwm_submap_id {
+ PWM_SUBMAP_PWM_A,
+ PWM_SUBMAP_PWM_B,
+ PWM_SUBMAP_SHAPE_EVAL,
+ PWM_SUBMAP_SP0_A,
+ PWM_SUBMAP_SP0_B,
+ PWM_SUBMAP_SP1_B,
+ PWM_SUBMAP_SP2_B,
+ PWM_SUBMAP_SP2_A,
+ PWM_SUBMAP_COUNT
+};
+
+/** Runtime-mutable descriptor array — bind with pwm_bind_submap_inputs. */
+extern pwm_submap_descr_t pwm_submap_descrs[PWM_SUBMAP_COUNT];
+
+#endif /* CAL_TABLES_ROM_H */
diff --git a/Core/Advance_Control/cal_tables_rom_504012.c b/Core/Advance_Control/cal_tables_rom_504012.c
new file mode 100644
index 0000000..7732253
--- /dev/null
+++ b/Core/Advance_Control/cal_tables_rom_504012.c
@@ -0,0 +1,138 @@
+/**
+ * @file cal_tables_rom.c (families/t06211/compact_src)
+ * @brief ROM-decoded t06211 calibration.
+ *
+ * AUTO-GENERATED by tools/extract_calibration.py
+ * Source ROM: rom_eeprom_dump_0000-9FFF_504012.bin
+ * Calibration base (RWA4): 0x9BD8
+ * Flash anchor: 0x7E18
+ * Generated: 2026-04-27 12:40:48
+ *
+ * DO NOT EDIT — regenerate with:
+ * python tools/extract_calibration.py --family t06211
+ */
+#include "pwm.h"
+
+/* ── Submap x/y arrays ──────────────────────────────────────────────── */
+
+static const int16_t setpoint_x[6] = { 8389, 5872, 4614, 2726, 1426, 0 };
+static const int16_t setpoint_y[6] = { 1707, 1707, 1707, 939, 427, 427 };
+
+static const int16_t pwm_A_x[9] = { 25166, 18455, 13841, 8389, 5872, 4614, 2726, 1426, 0 };
+static const int16_t pwm_A_y[9] = { 1707, 1365, 1195, 768, 427, 85, 0, -171, -469 };
+
+static const int16_t pwm_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 };
+
+static const int16_t pwm_y_table_rom[90] = {
+ /* row 0 */ 205, 205, 205, 205, 205, 205, 0, 0, 0,
+ /* row 1 */ 1638, 1392, 1229, 962, 778, 512, 0, 0, 0,
+ /* row 2 */ 1843, 1577, 1433, 1208, 1003, 758, 0, 0, 0,
+ /* row 3 */ 2457, 2109, 1986, 1761, 1577, 1229, 287, 0, 0,
+ /* row 4 */ 2867, 2744, 2641, 2457, 2314, 2129, 1269, 205, 205,
+ /* row 5 */ 3686, 3481, 3460, 3378, 3276, 3174, 2907, 1024, 737,
+ /* row 6 */ 3890, 3870, 3849, 3829, 3808, 3788, 3235, 1310, 1024,
+ /* row 7 */ 3890, 3890, 3890, 3890, 3890, 3890, 3890, 2076, 1638,
+ /* row 8 */ 3890, 3890, 3890, 3890, 3890, 3890, 3890, 3890, 3481,
+ /* row 9 */ 3890, 3890, 3890, 3890, 3890, 3890, 3890, 3890, 3890,
+};
+
+/* ── 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,
+ },
+ [PWM_SUBMAP_PWM_A] = {
+ .flags = 0, .input_ptr = NULL, .count = 9,
+ .x = pwm_A_x, .input_addr = 0x0040,
+ },
+ [PWM_SUBMAP_PWM_B] = {
+ .flags = 0, .input_ptr = NULL, .count = 10,
+ .x = pwm_B_x, .input_addr = 0x0046,
+ },
+ [PWM_SUBMAP_SHAPE_EVAL] = {
+ .flags = 0, .input_ptr = NULL, .count = 4,
+ .x = shape_x, .input_addr = 0x0142,
+ },
+};
+
+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 ────────────────────────────────────────────────────────── */
+
+const pwm_calibration_t pwm_cal_rom = {
+ .large_pos_error_thresh = 128, /* cal+0x10E */
+ .large_neg_error_thresh = (int16_t)0xFF00, /* cal+0x110 */
+ .pi_low_clamp = (int16_t)0xFE00, /* cal+0x120 */
+ .pi_high_clamp = 1707, /* cal+0x124 */
+
+ /* CAN-decoded setpoint (FUN_64c3) cal constants */
+ .b_fb_kw_upper_bound = 7680, /* cal+0x004 */
+ .b_fb_kw_lower_bound = (int16_t)0xFD00, /* cal+0x006 */
+ /* setpoint_offset = cal+0x4c - cal+0x4e = 3499 - 4156 = -657 */
+ .setpoint_offset = (int16_t)0xFD6F,
+ .target_5e_min_clamp = (int16_t)0xFE00, /* cal+0x122 */
+ .can_aux_12e_max = 1451, /* cal+0x002 */
+ .error_thresh_114 = 100, /* cal+0x114 */
+ .pi_thresh_116 = 96, /* cal+0x116 */
+ .pi_sat_count_threshold = 800, /* cal+0x112 = 0x0320 */
+ .rpm_threshold_11E = 2936, /* cal+0x11E */
+ .pi_cl_rpm_floor = 420, /* RAM[0x605c] = 0x01A4 */
+
+ .pwm_detail_x0 = (int16_t)0x9C40, /* cal+0x0EE */
+ .pwm_detail_x1 = (int16_t)0x8235, /* cal+0x0F0 */
+ .pwm_cached_ptr_0F2 = 5662, /* cal+0x0F2 (legacy scalar) */
+ .pwm_cached_ptr_102 = 168, /* cal+0x102 (legacy scalar) */
+
+ /* RPM-window matching: inline 8-int16 band array at cal+0x0F2..0x100
+ * (4 (lo,hi) pairs) plus halfwidth at cal+0x102. Drives the three-
+ * phase pwm_period slew in FUN_5314 (open-questions §5 closeout). */
+ .pwm_rpm_windows = { 5662, 6795, 8808, 10486, 11954, 13422, 18036, 19713 },
+ .pwm_window_halfwidth = 168,
+
+ /* pwm_const_104 / pwm_slew_step are the same value (cal+0x104 = 354);
+ * pwm_const_104 retained for backward compat, pwm_slew_step is the
+ * semantic name used by the RPM-window matcher (per-cycle pwm_period
+ * slew magnitude, applied at Phase 1 / Phase 3). Cached to
+ * RAM[0x02ec] at FUN_5314:0x53b9. */
+ .pwm_const_104 = 354, /* cal+0x104 (legacy alias) */
+ .pwm_slew_step = 354, /* cal+0x104 (semantic) */
+
+ .pwm_y_table = pwm_y_table_rom, /* cal+0x154 @ 0x9E28 */
+ .shape_y_table = shape_y_table_rom, /* cal+0x15E @ 0x9E20 */
+
+ /* CL-correction gain — cached at absolute ROM[0x6056], not cal-relative. */
+ .closed_loop_gain_const = 10, /* ROM[0x6056] */
+
+ /* PWM period endpoints (alias of pwm_detail_x0/x1) as unsigned. */
+ .pwm_period_min = 33333, /* cal+0x0F0 */
+ .pwm_period_max = 40000, /* cal+0x0EE */
+
+ /* Duty clamp bounds — RAM[0x6058]/RAM[0x605a] mirrors. Same defaults
+ * as the default family's pwm_min/pwm_max. */
+ .pwm_min = 0x00CD, /* RAM[0x6058] = 205 */
+ .pwm_max = 0x0F32, /* RAM[0x605a] = 3890 */
+};
+
+/* Family-1 API parity placeholder — t06211 keeps Y-tables inside
+ * pwm_cal_rom, so pwm_flash_rom has no data. Callers pass &pwm_flash_rom
+ * to pwm_init() purely for signature compatibility. */
+const pwm_flash_t pwm_flash_rom = { 0 };
diff --git a/Core/Advance_Control/cal_tables_rom_504012.h b/Core/Advance_Control/cal_tables_rom_504012.h
new file mode 100644
index 0000000..54682cc
--- /dev/null
+++ b/Core/Advance_Control/cal_tables_rom_504012.h
@@ -0,0 +1,12 @@
+/**
+ * @file cal_tables_rom.h (families/t06211/compact_src)
+ * @brief Extern decls for the ROM-decoded t06211 calibration.
+ * All declarations live in pwm.h; this file just re-exports the enum
+ * and array symbols for the compact_src/pwm.c translation unit.
+ */
+#ifndef CAL_TABLES_ROM_T06211_COMPACT_H
+#define CAL_TABLES_ROM_T06211_COMPACT_H
+
+#include "pwm.h"
+
+#endif /* CAL_TABLES_ROM_T06211_COMPACT_H */
diff --git a/Core/Advance_Control/pwm.c b/Core/Advance_Control/pwm.c
index 7d36c8c..30472b9 100644
--- a/Core/Advance_Control/pwm.c
+++ b/Core/Advance_Control/pwm.c
@@ -1,69 +1,81 @@
/**
- * @file pwm.c
- * @brief Compact single-file implementation of the VP44 PWM solenoid control
- * pipeline. Combines interpolation, setpoint, supervisor, PI, PWM
- * output, orchestrator, ported ex-external routines, and default
- * cal data.
+ * @file pwm.c (families/t06211/compact_src)
+ * @brief Compact single-file implementation of the t06211 PWM control
+ * pipeline. Merges the 10 per-module files from
+ * families/t06211/src/ into one translation unit.
*
- * Every arithmetic choice (MUL vs MULU, SHRA vs SHR, JGE vs JC) mirrors the
- * MCS-96 assembly — see the original src/*.c files for per-block address
- * annotations and disassembly/*.txt for the source.
+ * Pipeline (t06211 FUN_77b3 @ 0x77b3):
+ * 1. setpoint (FUN_7168) — single-submap RPM-indexed interp
+ * 2. supervisor (FUN_7beb) — reset + counter + error + clamp
+ * 3. publish_cl (FUN_7cd8) — calls cl_correction, publishes est_angle
+ * 4. pi_controller (FUN_67c4) — open/closed-loop branch
+ * 5. pwm_output (FUN_5314) — eval+eval+combine+saturate+HW shadow
+ *
+ * Per-block address citations follow the families/t06211/src/ per-module
+ * ports verbatim; see those files for expanded commentary.
*/
#include "pwm.h"
-#include "cal_tables_rom.h"
-/* Forward decls for ported ex-external routines (defined below). */
-static int16_t s_eval (const pwm_submap_descr_t *descr,
- pwm_interp_slot_t *slot);
-static int16_t s_combine (const int16_t *y_base,
- const pwm_interp_slot_t *sx,
- const pwm_interp_slot_t *sy);
-static int16_t s_refine (const int16_t *y_base,
- const pwm_interp_slot_t *slot);
-static void s_correction(pwm_runtime_t *rt,
- const pwm_calibration_t *cal);
-static void s_recovery (pwm_runtime_t *rt,
- const pwm_calibration_t *cal,
- uint16_t rpm);
+/* Forward decls for file-static helpers. */
+static void s_eval_submap(const pwm_submap_descr_t *d,
+ pwm_interp_slot_t *slot);
+static int16_t s_combine(const int16_t *y_base,
+ const pwm_interp_slot_t *sa,
+ const pwm_interp_slot_t *sb);
+static int16_t s_refine(const int16_t *y_base,
+ const pwm_interp_slot_t *slot);
+
+static void s_setpoint (pwm_runtime_t *rt);
+static void s_supervisor (pwm_runtime_t *rt);
+static void s_cl_correct (pwm_runtime_t *rt,
+ const pwm_calibration_t *cal);
+static void s_publish_cl (pwm_runtime_t *rt,
+ const pwm_calibration_t *cal);
+static void s_pi_update (pwm_runtime_t *rt,
+ const pwm_calibration_t *cal);
+static void s_pwm_output (pwm_runtime_t *rt,
+ const pwm_calibration_t *cal);
/* ═════════════════════════════════════════════════════════════════════
- * Runtime init (file-static — exposed via pwm_init)
+ * Init / binding
* ═════════════════════════════════════════════════════════════════════ */
+
static void runtime_reset(pwm_runtime_t *rt)
{
- memset(rt, 0, sizeof *rt);
- rt->mode_flags = 0x03; /* first_call + outer */
- rt->min_rpm_openloop = 0x01A4; /* 420 RPM */
- rt->upper_breakpoint = 107;
- rt->lower_breakpoint = (int16_t)0xFF95;
- rt->center_slope = 2560;
- rt->upper_outer_slope = 5120;
- rt->lower_outer_slope = 5120;
- rt->upper_integrator_gain = 1024;
- rt->center_integrator_gain = 1024;
- rt->lower_integrator_gain = 2048;
- rt->pwm_period = 29851;
- rt->shape_scale = 24;
- rt->pwm_min = 205;
- rt->pwm_max = 3890;
- /* Observed defaults from Ghidra annotations on compute_closed_loop_correction
- * (0x01F4, 0x028A) and fast_recovery (0x01F4). TODO: locate flash source. */
- rt->pos_error_normalizer = 500; /* 0x01F4 */
- rt->neg_error_normalizer = 650; /* 0x028A */
- rt->recovery_count_threshold = 500; /* 0x01F4 */
+ 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_calibration(pwm_runtime_t *rt, const pwm_calibration_t *c)
+static void apply_cal(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal)
{
- rt->upper_breakpoint = c->pi_upper_breakpoint;
- rt->lower_breakpoint = c->pi_lower_breakpoint;
- rt->center_slope = c->pi_center_slope;
- rt->upper_outer_slope = c->pi_upper_outer_slope;
- rt->lower_outer_slope = c->pi_lower_outer_slope;
- rt->upper_integrator_gain = c->pi_upper_integrator_gain;
- rt->center_integrator_gain = c->pi_center_integrator_gain;
- rt->lower_integrator_gain = c->pi_lower_integrator_gain;
- rt->setpoint_offset = c->setpoint_offset;
+ /* Mirrors family-1's pwm_init copy of cal→runtime statics. */
+ rt->setpoint_offset_150 = cal->setpoint_offset;
}
void pwm_init(pwm_runtime_t *rt,
@@ -71,14 +83,11 @@ void pwm_init(pwm_runtime_t *rt,
const pwm_flash_t *flash,
const pwm_input_getters_t *getters)
{
+ (void)flash; /* family-1 API parity; t06211 keeps Y-tables in cal */
runtime_reset(rt);
- apply_calibration(rt, cal);
rt->bound_cal = cal;
- rt->bound_flash = flash;
rt->bound_getters = getters;
- /* Resolve each descriptor's input_ptr into this rt's inputs block, so
- * s_eval() can dereference live values. Descriptor order/IDs are fixed
- * by the PWM_SUBMAP_* enum in cal_tables_rom.h. */
+ apply_cal(rt, cal);
pwm_bind_submap_inputs(rt, pwm_submap_descrs, PWM_SUBMAP_COUNT);
}
@@ -88,456 +97,643 @@ static void read_inputs(pwm_runtime_t *rt)
void *ctx = g->ctx;
rt->inputs.ckp_in = g->ckp_in (ctx);
rt->inputs.rpm = g->rpm (ctx);
- rt->inputs.angle_dec_cmd = g->angle_dec_cmd (ctx);
+ rt->inputs.angle_dec_cmd = g->angle_dec_cmd (ctx); /* accepted; unused */
rt->inputs.inj_qty_demand = g->inj_qty_demand(ctx);
rt->inputs.b_fb_kw = g->b_fb_kw (ctx);
rt->inputs.state_130 = g->state_130 (ctx);
rt->inputs.supply_voltage = g->supply_voltage(ctx);
- rt->inputs.temperature = g->temperature (ctx);
+ rt->inputs.temperature = g->temperature (ctx); /* accepted; unused */
+
+ /* The b_fb_kw getter result drives the CAN-decoded setpoint chain
+ * (FUN_64c3). Mirrors the real ROM where RAM[0x12c] is written by
+ * the CAN parser before FUN_64c3 runs. */
+ rt->can_raw_b_fb_kw = rt->inputs.b_fb_kw;
}
/* ═════════════════════════════════════════════════════════════════════
- * Interpolation — helper_from_cal (0x838b–0x83fa)
- * Descending-X piecewise linear, signed MUL + signed DIV.
+ * Interpolation — FUN_7168 (0x7168-0x71d7), fingerprint #3
+ * Raw helper; also drives the setpoint stage via descriptor.
* ═════════════════════════════════════════════════════════════════════ */
+
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)
{
- if (in >= x[0]) return y[0];
- if (in <= x[n - 1u]) return y[n - 1u];
+ if (in >= x[0]) return y[0];
+ if (in <= x[n - 1u]) return y[n - 1u];
- /* One-step binary midpoint jump, then linear scan. */
- uint16_t mid = (n & 0xFFFEu) / 2u;
- uint16_t k = (in < x[mid]) ? (uint16_t)(mid + 1u) : 1u;
- while (k < n && in < x[k]) k++;
+ uint16_t k = 1u;
+ while (k < n && in < x[k]) { k++; }
- int16_t num = (int16_t)(in - x[k]);
- int16_t dx = (int16_t)(x[k] - x[k - 1u]);
+ int16_t x_k = x[k];
+ int16_t num = (int16_t)(in - x_k);
+ int16_t dx = (int16_t)(x_k - x[k - 1u]);
int16_t dy = (int16_t)(y[k] - y[k - 1u]);
+
int32_t prod = MUL_S16(num, dy);
- return (int16_t)((int16_t)(prod / (int32_t)dx) + y[k]);
+ 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]);
}
/* ═════════════════════════════════════════════════════════════════════
- * Setpoint — precompute_control_support_maps (0x8e36–0x8f1f)
+ * Submap eval / bilinear combine / refine — t06211 FUN_6fb8 / FUN_7035 /
+ * FUN_7014. Scratch layout matches the family-1 ROM convention.
* ═════════════════════════════════════════════════════════════════════ */
-static void setpoint_compute(pwm_runtime_t *rt, const pwm_calibration_t *cal)
+
+static void s_eval_submap(const pwm_submap_descr_t *d,
+ pwm_interp_slot_t *slot)
{
- /* Pair 0: slot_x=sp0_A(rpm), slot_y=sp0_B(inj_qty_demand) */
- s_eval(&pwm_submap_descrs[PWM_SUBMAP_SP0_A], &rt->setpoint_slot_x);
- s_eval(&pwm_submap_descrs[PWM_SUBMAP_SP0_B], &rt->setpoint_slot_y);
- rt->compensation_angle =
- s_combine(cal->y_pair[0], &rt->setpoint_slot_x, &rt->setpoint_slot_y);
+ int16_t input = d->input_ptr ? *d->input_ptr : 0;
+ slot->row_stride = (int16_t)(d->count * 2u);
- /* Pair 1: slot_x reused (still sp0_A rpm), slot_y=sp1_B(temperature) */
- s_eval(&pwm_submap_descrs[PWM_SUBMAP_SP1_B], &rt->setpoint_slot_y);
- rt->compensation_angle = (int16_t)(rt->compensation_angle +
- s_combine(cal->y_pair[1], &rt->setpoint_slot_x, &rt->setpoint_slot_y));
-
- /* Pair 2: slot_x=sp2_A(rpm), slot_y=sp2_B(angle_dec_cmd) */
- s_eval(&pwm_submap_descrs[PWM_SUBMAP_SP2_A], &rt->setpoint_slot_x);
- s_eval(&pwm_submap_descrs[PWM_SUBMAP_SP2_B], &rt->setpoint_slot_y);
- rt->compensation_angle = (int16_t)(rt->compensation_angle +
- s_combine(cal->y_pair[2], &rt->setpoint_slot_x, &rt->setpoint_slot_y));
-
- int16_t sum = (int16_t)(rt->inputs.b_fb_kw + rt->compensation_angle);
- int16_t half = shra16(sum, 1); /* SHRA — signed halve */
- rt->pre_offset_target = half;
-
- int16_t t = (int16_t)(half + rt->setpoint_offset);
- if (t < cal->target_minimum) t = cal->target_minimum;
- rt->target = t;
-}
-
-/* ═════════════════════════════════════════════════════════════════════
- * Supervisor — update_closed_loop_supervisor (0x929b–0x92f1)
- * One-sided (upper-only) clamp on cl_error_delta.
- * ═════════════════════════════════════════════════════════════════════ */
-static void supervisor_update(pwm_runtime_t *rt)
-{
- if (rt->reset_flag == 1) {
- rt->reset_flag = 0;
- rt->cl_enable_counter = 0;
- rt->supervisor_state_17e = 0;
- /* ROM snapshots integrator_hi → 0x033e here; confirmed dead-store, dropped */
- } else {
- rt->cl_enable_counter = (int16_t)(rt->cl_enable_counter + 1);
- }
- int16_t err = (int16_t)(rt->target - rt->inputs.ckp_in);
- err = (int16_t)(err + rt->angle_error_shaped);
- rt->cl_error_delta = err;
- if (err > rt->max_cl_error) rt->cl_error_delta = rt->max_cl_error;
-}
-
-/* ═════════════════════════════════════════════════════════════════════
- * PI controller — finalize_angle_request (0x713b–0x7414)
- * ═════════════════════════════════════════════════════════════════════ */
-static void shape_error(pwm_runtime_t *rt)
-{
- int16_t e = rt->angle_error;
- int16_t bp, slope, gain;
-
- if (e > rt->upper_breakpoint) {
- rt->mode_flags |= MODE_OUTER_REGION;
- bp = rt->upper_breakpoint; slope = rt->upper_outer_slope;
- gain = rt->upper_integrator_gain;
- } else if (e < rt->lower_breakpoint) {
- rt->mode_flags |= MODE_OUTER_REGION;
- bp = rt->lower_breakpoint; slope = rt->lower_outer_slope;
- gain = rt->lower_integrator_gain;
- } else {
- rt->mode_flags &= (uint8_t)~MODE_OUTER_REGION;
- rt->integrator_gain = rt->center_integrator_gain;
- rt->angle_error_shaped =
- (int16_t)shra32(MUL_S16(rt->center_slope, e), 8);
+ if (d->count == 0 || d->x == NULL) {
+ slot->x_interval = 2;
+ slot->x_offset = 2;
+ slot->y_byte_off = 2;
return;
}
- rt->integrator_gain = gain;
- int32_t outer = MUL_S16((int16_t)(e - bp), slope);
- int32_t center = MUL_S16(rt->center_slope, bp);
- rt->angle_error_shaped = (int16_t)shra32(outer + center, 8);
-}
-static void pi_controller_update(pwm_runtime_t *rt,
- const pwm_calibration_t *cal,
- const pwm_flash_t *flash)
-{
- const uint16_t rpm = rt->inputs.rpm;
- const int16_t inj_qty_demand = rt->inputs.inj_qty_demand;
-
- /* A. Entry conditions — all three must hold for closed-loop. */
- bool open_loop = (rt->system_flags_110 & 0x20) != 0
- || rt->inputs.state_130 < 0
- || (uint16_t)rpm < (uint16_t)rt->min_rpm_openloop;
-
- if (open_loop) {
- rt->mode_flags |= MODE_FIRST_CALL;
- rt->active_request = rt->target;
- if (rt->active_request < flash->request_upper_limit)
- rt->active_request = flash->request_upper_limit;
- goto rotate_flags;
- }
-
- /* C. Error */
- rt->angle_error = (int16_t)(rt->target - rt->estimated_angle);
-
- /* D/E/F. Large-positive / large-negative / normal-range paths */
- if (rt->angle_error > flash->large_pos_error_thresh) {
- rt->mode_flags = (uint8_t)((rt->mode_flags | MODE_LARGE_POS) & ~MODE_LARGE_NEG);
- s_recovery(rt, cal, rpm);
- } else if (rt->angle_error < flash->large_neg_error_thresh) {
- rt->mode_flags = (uint8_t)((rt->mode_flags | MODE_LARGE_NEG) & ~MODE_LARGE_POS);
- if ((uint16_t)inj_qty_demand > (uint16_t)flash->inj_qty_demand_thresh)
- s_recovery(rt, cal, rpm);
- else
- rt->recovery_counter = 0;
- } else {
- rt->mode_flags &= (uint8_t)~(MODE_LARGE_POS | MODE_LARGE_NEG);
- if ((uint16_t)rt->error_persist_counter > 0u)
- rt->error_persist_counter = (int16_t)(rt->error_persist_counter - 1);
- if (rt->error_persist_counter == 0)
- rt->system_flags_110 &= (uint8_t)~0x01;
- if (rt->recovery_counter > 0)
- rt->recovery_counter = (int16_t)(rt->recovery_counter - 1);
- }
-
- /* G. Shape error */
- shape_error(rt);
-
- /* H. Integrator — first-call init or anti-windup-gated accumulate */
- if (rt->mode_flags & MODE_FIRST_CALL) {
- int16_t lo_prod = (int16_t)MUL_S16(rt->first_call_gain, rt->angle_error);
- int16_t init_hi = (int16_t)(shra16(lo_prod, 4) + rt->inputs.ckp_in);
- rt->integrator_state = (int32_t)(((uint32_t)(uint16_t)init_hi << 16) |
- INTEG_LO(rt->integrator_state));
- rt->mode_flags &= (uint8_t)~MODE_FIRST_CALL;
- } else {
- bool freeze = (rt->angle_error < 0)
- ? (rt->mode_flags & MODE_NEG_SAT) != 0
- : (rt->mode_flags & MODE_POS_SAT) != 0;
- if (!freeze) {
- int32_t inc = MUL_S16(rt->angle_error, rt->integrator_gain) << 4;
- rt->integrator_state += inc;
- }
- }
-
- /* I. Output + saturation clamps (signed) */
- rt->active_request = (int16_t)(rt->angle_error_shaped +
- INTEG_HI(rt->integrator_state));
- if (rt->active_request < flash->request_upper_limit) {
- rt->active_request = flash->request_upper_limit;
- rt->mode_flags = (uint8_t)((rt->mode_flags | MODE_NEG_SAT) & ~MODE_POS_SAT);
- } else if (rt->active_request > rt->max_cl_error) {
- rt->active_request = rt->max_cl_error;
- rt->mode_flags = (uint8_t)((rt->mode_flags | MODE_POS_SAT) & ~MODE_NEG_SAT);
- } else {
- rt->mode_flags &= (uint8_t)~(MODE_NEG_SAT | MODE_POS_SAT);
- }
-
-rotate_flags:
- /* J. Rotate bits 4-5 into bits 6-7 (prev_sat_shadow). Consumed next
- * cycle by fast_recovery (FUN_70ae at 0x70ae/0x70b6) via the pattern
- * sustained = (mode_flags << 2) & mode_flags
- * to detect two-cycle sustained saturation. [0x73f4–0x7414] */
- rt->mode_flags = (uint8_t)((rt->mode_flags & 0x3Fu) |
- (((uint16_t)rt->mode_flags << 2) & 0xC0u));
-}
-
-/* ═════════════════════════════════════════════════════════════════════
- * PWM output — compute_pwm_duty_from_maps (0x7415–0x7760)
- * Nearly all UNSIGNED arithmetic (MULU, SHR, DIVU, JC/JNH).
- * ═════════════════════════════════════════════════════════════════════ */
-static bool rpm_in_any_window(uint16_t rpm, const uint16_t *bp,
- uint16_t lo_pad, uint16_t hi_pad)
-{
- for (int i = 0; i < 4; i++) {
- uint16_t lo = (uint16_t)(bp[2 * i] - lo_pad);
- uint16_t hi = (uint16_t)(bp[2 * i + 1] + hi_pad);
- if (rpm > lo && rpm < hi) return true;
- }
- return false;
-}
-
-static void pwm_output_compute(pwm_runtime_t *rt, const pwm_flash_t *flash)
-{
- const uint16_t rpm = rt->inputs.rpm;
-
- /* A. Base duty from submap pair — slot_x=pwm_A(rpm),
- * slot_y=pwm_B(active_request) [0x0046 = PI output feed-forward] */
- s_eval(&pwm_submap_descrs[PWM_SUBMAP_PWM_A], &rt->pwm_slot_x);
- s_eval(&pwm_submap_descrs[PWM_SUBMAP_PWM_B], &rt->pwm_slot_y);
- rt->pwm_duty = (uint16_t)s_combine(
- flash->pwm_y_table, &rt->pwm_slot_x, &rt->pwm_slot_y);
-
- /* B. Duty status flag (unsigned) */
- rt->pwm_status_flag = (rt->pwm_duty < 0x29u) ? 1u
- : (rt->pwm_duty > 0xFD7u) ? 2u : 0u;
-
- /* C/D. Coarse RPM window — shape_intermediate = +shape_scale if in any */
- rt->shape_scale = flash->shape_scale_src;
- bool coarse = rpm_in_any_window(rpm, flash->rpm_breakpoints, 0, 0);
-
- if (coarse) {
- rt->shape_intermediate = rt->shape_scale;
- } else {
- /* E. Detail pass — expanded windows padded by rpm_values[0] */
- uint16_t pad = flash->rpm_values[0];
- bool detail = rpm_in_any_window(rpm, flash->rpm_breakpoints, pad, pad);
- if (!detail && rt->pwm_period < flash->period_max_limit)
- rt->shape_intermediate = (int16_t)(-rt->shape_scale);
- /* else: shape_intermediate unchanged */
- }
-
- /* F. Period adjust + unsigned clamp to [min, max] */
- rt->pwm_period = (uint16_t)(rt->pwm_period - (uint16_t)rt->shape_intermediate);
- if (rt->pwm_period > flash->period_max_limit) rt->pwm_period = flash->period_max_limit;
- if (rt->pwm_period < flash->period_min_limit) rt->pwm_period = flash->period_min_limit;
-
- /* G. Shape refinement (signed clamp to [0, 0x199]) — shape_eval(supply_voltage) */
- s_eval(&pwm_submap_descrs[PWM_SUBMAP_SHAPE_EVAL], &rt->pwm_slot_x);
- rt->shape_output = s_refine(flash->shape_y_table, &rt->pwm_slot_x);
- if (rt->shape_output < 0) rt->shape_output = 0;
- if (rt->shape_output > (int16_t)0x199) rt->shape_output = (int16_t)0x199;
-
- /* H. Duty refinement — all UNSIGNED (SHR, MULU, DIVU) */
- uint16_t range = (uint16_t)((uint16_t)(flash->period_max_limit
- - flash->period_min_limit) >> 8);
- rt->shape_scale = (int16_t)range;
- uint16_t delta = (uint16_t)((uint16_t)(flash->period_max_limit
- - rt->pwm_period) >> 8);
- uint32_t prod = MULU_U16(delta, (uint16_t)rt->shape_output);
- uint16_t trunc = (uint16_t)(prod & 0xFFFFu); /* assembly CLR RW1E */
- uint16_t quot = range ? (uint16_t)(trunc / range) : 0u;
- rt->pwm_duty = (uint16_t)(rt->pwm_duty + quot);
-
- /* I. Absolute duty clamp (unsigned) */
- if (rt->pwm_duty < rt->pwm_min) rt->pwm_duty = rt->pwm_min;
- if (rt->pwm_duty > rt->pwm_max) rt->pwm_duty = rt->pwm_max;
-
- /* J. HW register split — critical section in assembly (DI/EI) */
- uint32_t hw = MULU_U16(rt->pwm_period, rt->pwm_duty);
- rt->pwm_on_time = (uint16_t)(hw / 0xFFFu);
- rt->pwm_off_time = (uint16_t)(rt->pwm_period - rt->pwm_on_time);
-}
-
-/* ═════════════════════════════════════════════════════════════════════
- * Orchestrator — main_pwm_control_service (0x8cc9–0x8cf1)
- * + inlined publish_closed_loop_request (0x937d–0x938f)
- * ═════════════════════════════════════════════════════════════════════ */
-void pwm_service(pwm_runtime_t *rt)
-{
- /* 0. Refresh external inputs via user getters. */
- read_inputs(rt);
-
- const pwm_calibration_t *cal = rt->bound_cal;
- const pwm_flash_t *flash = rt->bound_flash;
-
- /* 1. Max-error interpolation */
- if (flash->max_error_count > 0)
- rt->max_cl_error = pwm_interp_lookup(flash->max_error_x,
- flash->max_error_y,
- flash->max_error_count,
- (int16_t)rt->inputs.rpm);
- /* 2. Target setpoint */
- setpoint_compute(rt, cal);
-
- /* 3. Supervisor */
- supervisor_update(rt);
-
- /* 4. Publish correction + angle estimate */
- s_correction(rt, cal);
- rt->estimated_angle = (int16_t)(rt->inputs.ckp_in + rt->angle_offset);
-
- /* 5. PI controller */
- pi_controller_update(rt, cal, flash);
-
- /* 6. PWM duty + HW registers */
- pwm_output_compute(rt, flash);
-}
-
-/* ═════════════════════════════════════════════════════════════════════
- * External function impls (ported 1:1 from disassembly)
- * ═════════════════════════════════════════════════════════════════════ */
-/* real_eval_submap (disassembled 81db–8236). */
-static int16_t s_eval(const pwm_submap_descr_t *descr, pwm_interp_slot_t *slot)
-{
- int16_t input_val = (descr && descr->input_ptr) ? *descr->input_ptr : 0;
- uint16_t count = descr ? descr->count : 0u;
- const int16_t *x = descr ? descr->x : NULL;
- if (count == 0u || x == NULL) {
- memset(slot, 0, sizeof *slot);
- return 0;
- }
- slot->row_stride = (int16_t)(count * 2u);
uint16_t k;
- for (k = 1; k < count; k++) {
- if (input_val >= x[k]) break;
+ for (k = 1u; k < d->count; k++) {
+ if (input >= d->x[k]) break;
}
- if (k == 1 && input_val >= x[0]) {
- slot->x_interval = 2; slot->x_offset = 2; slot->y_byte_off = 2;
- return 0;
+
+ if (k == 1u && input >= d->x[0]) {
+ /* Upper-clamp sentinel [_, 2, 2, 2] */
+ slot->x_interval = 2;
+ slot->x_offset = 2;
+ slot->y_byte_off = 2;
+ return;
}
- if (k >= count) k = (uint16_t)(count - 1u);
- slot->x_interval = (int16_t)(x[k - 1] - x[k]);
- slot->x_offset = (int16_t)(input_val - x[k]);
+ if (k >= d->count) { k = (uint16_t)(d->count - 1u); }
+
+ slot->x_interval = (int16_t)(d->x[k - 1u] - d->x[k]);
+ slot->x_offset = (int16_t)(input - d->x[k]);
slot->y_byte_off = (int16_t)(k * 2u);
- return 0;
}
-/* real_combine_submaps (disassembled 8258–82b4). Bilinear over row-major
- * [B_count][A_count] int16 Y-table. slot_x->row_stride is A-axis byte stride. */
static int16_t s_combine(const int16_t *y_base,
- const pwm_interp_slot_t *sx,
- const pwm_interp_slot_t *sy)
+ const pwm_interp_slot_t *sa,
+ const pwm_interp_slot_t *sb)
{
- if (y_base == NULL || sx->x_interval == 0 || sy->x_interval == 0) return 0;
- int32_t row_off = MUL_S16(sy->y_byte_off, sx->row_stride) / 2;
- const int16_t *yp = (const int16_t *)((const uint8_t *)y_base
- + row_off + sx->y_byte_off);
- int16_t y_here = *yp;
- int16_t y_prev = *(const int16_t *)((const uint8_t *)yp - 2);
- int32_t diff_a = MUL_S16((int16_t)(y_prev - y_here), sx->x_offset);
- int16_t rowB = (int16_t)(y_here + (int32_t)(diff_a / (int32_t)sx->x_interval));
+ if (!y_base || !sa || !sb) return 0;
- const int16_t *yp_p = (const int16_t *)((const uint8_t *)yp - sx->row_stride);
- int16_t y_here_p = *yp_p;
- int16_t y_prev_p = *(const int16_t *)((const uint8_t *)yp_p - 2);
- int32_t diff_a_p = MUL_S16((int16_t)(y_prev_p - y_here_p), sx->x_offset);
- int16_t rowBp = (int16_t)(y_here_p
- + (int32_t)(diff_a_p / (int32_t)sx->x_interval));
+ int16_t den_a = sa->x_interval ? sa->x_interval : 1;
+ int16_t den_b = sb->x_interval ? sb->x_interval : 1;
- int32_t diff_b = MUL_S16((int16_t)(rowBp - rowB), sy->x_offset);
- return (int16_t)(rowB + (int32_t)(diff_b / (int32_t)sy->x_interval));
+ int32_t row_off = MUL_S16(sb->y_byte_off, sa->row_stride) / 2;
+ const uint8_t *yp_b = (const uint8_t *)y_base + row_off + sa->y_byte_off;
+ const int16_t *yp = (const int16_t *)yp_b;
+
+ int16_t y_here = *yp;
+ int16_t y_prev_a = *(const int16_t *)(yp_b - 2);
+ int32_t diff_a = MUL_S16((int16_t)(y_prev_a - y_here), sa->x_offset);
+ int16_t rowB = (int16_t)(y_here + (int32_t)(diff_a / (int32_t)den_a));
+
+ const uint8_t *yp_b_prev = yp_b - sa->row_stride;
+ const int16_t *yp_prev = (const int16_t *)yp_b_prev;
+ int16_t y_here_pb = *yp_prev;
+ int16_t y_prev_a_pb = *(const int16_t *)(yp_b_prev - 2);
+ int32_t diff_a_pb = MUL_S16((int16_t)(y_prev_a_pb - y_here_pb), sa->x_offset);
+ int16_t rowBp = (int16_t)(y_here_pb + (int32_t)(diff_a_pb / (int32_t)den_a));
+
+ int32_t diff_b = MUL_S16((int16_t)(rowBp - rowB), sb->x_offset);
+ return (int16_t)(rowB + (int32_t)(diff_b / (int32_t)den_b));
}
-/* compute_closed_loop_correction (disassembled 92f2–9339). */
-static void s_correction(pwm_runtime_t *rt, const pwm_calibration_t *cal)
+static int16_t s_refine(const int16_t *y_base,
+ const pwm_interp_slot_t *slot)
{
- int16_t correction = 0;
- if ((uint8_t)(rt->cl_enable_counter & 0xFF) != 0) {
- int16_t normalizer = (rt->cl_error_delta > 0)
+ if (!y_base || !slot) return 0;
+ int16_t den = slot->x_interval ? slot->x_interval : 1;
+
+ const uint8_t *yp_b = (const uint8_t *)y_base + slot->y_byte_off;
+ const int16_t *yp = (const int16_t *)yp_b;
+ int16_t y_here = *yp;
+ int16_t y_prev_a = *(const int16_t *)(yp_b - 2);
+ int32_t diff = MUL_S16((int16_t)(y_prev_a - y_here), slot->x_offset);
+ return (int16_t)(y_here + (int32_t)(diff / (int32_t)den));
+}
+
+/* Descriptor-driven wrapper used only by the setpoint stage. */
+static int16_t s_interp_descr(const pwm_submap_descr_t *d,
+ const int16_t *y_array)
+{
+ int16_t input = d->input_ptr ? *d->input_ptr : 0;
+ return pwm_interp_lookup(d->x, y_array, d->count, input);
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Stage 1 — Setpoint (FUN_77b3:77b3-77c7 + FUN_7168)
+ * ═════════════════════════════════════════════════════════════════════ */
+
+static void s_setpoint(pwm_runtime_t *rt)
+{
+ const pwm_submap_descr_t *d =
+ &pwm_submap_descrs[PWM_SUBMAP_SETPOINT_INTERP];
+ const int16_t *y = pwm_submap_y_of(PWM_SUBMAP_SETPOINT_INTERP);
+ rt->target_336 = s_interp_descr(d, y);
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Stage 1b — CAN-decoded setpoint (FUN_64c3 @ 0x64c3-0x650a)
+ * Real ROM call site: FUN_6192:0x61cc (CAN parser dispatcher).
+ * For the C model, invoked from pwm_service after read_inputs so the
+ * harness's per-cycle b_fb_kw value flows into target before PI runs.
+ * Skip the RE7 gate and error path (sim trusts caller).
+ * ═════════════════════════════════════════════════════════════════════ */
+
+static void s_setpoint_can_decode(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal)
+{
+ int16_t raw = rt->can_raw_b_fb_kw;
+ /* [0x64ca-0x64d6] Bounds check */
+ if (raw > cal->b_fb_kw_upper_bound) return;
+ if (raw < cal->b_fb_kw_lower_bound) return;
+
+ /* [0x64dd] half = raw >> 1 (SHRA — sign-extending) */
+ int16_t half = shra16(raw, 1);
+ rt->can_half_12a = half;
+
+ /* [0x64e5-0x64ea] result = half + RAM[0x150] + RW42 */
+ int16_t result = (int16_t)(half + rt->setpoint_offset_150 + rt->rw42_state);
+
+ /* [0x64ed-0x64f9] Lower clamp */
+ if (result < cal->target_5e_min_clamp) {
+ result = cal->target_5e_min_clamp;
+ }
+ rt->target_5e = result;
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Stage 2 — Supervisor (FUN_7beb @ 0x7beb-0x7c41)
+ * ═════════════════════════════════════════════════════════════════════ */
+
+static void s_supervisor(pwm_runtime_t *rt)
+{
+ /* Reset-flag branch (0x7beb-0x7c0c) */
+ if (rt->reset_flag == 1u) {
+ rt->reset_flag = 0u;
+ rt->cl_enable_counter = 0u;
+ rt->supervisor_state_17e = 0;
+ rt->b_fb_kw_baseline = rt->inputs.b_fb_kw;
+ } else {
+ /* Counter tick (0x7c11-0x7c1b) */
+ rt->cl_enable_counter = (uint16_t)(rt->cl_enable_counter + 1u);
+ }
+
+ /* Error compute (0x7c20-0x7c2b)
+ * Disasm: SUB RW1C, RW5E, DAT_02f8 — primary setpoint is target. */
+ int16_t error = (int16_t)(rt->target_5e - rt->inputs.ckp_in);
+ error = (int16_t)(error + rt->compensation_angle);
+ rt->angle_error_raw = error;
+
+ /* Ceiling clamp (0x7c30-0x7c41)
+ * Disasm: CMP RW1C, DAT_0336 — clamp uses RPM-derived ceiling. */
+ if (error > rt->target_336) {
+ rt->angle_error_raw = rt->target_336;
+ }
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Stage 3a — Closed-loop correction (FUN_5f1f @ 0x5f1f-0x5f66)
+ * Fingerprint #10 bit-for-bit match to family-1 algorithm.
+ * ═════════════════════════════════════════════════════════════════════ */
+
+static void s_cl_correct(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal)
+{
+ int16_t correction;
+
+ /* Gate on low byte of cl_enable_counter (0x5f1f-0x5f24). */
+ if ((rt->cl_enable_counter & 0xFFu) == 0u) {
+ correction = 0;
+ } else {
+ int16_t normalizer = (rt->angle_error_raw > 0)
? rt->pos_error_normalizer
: rt->neg_error_normalizer;
- int32_t product = MUL_S16(rt->cl_error_delta,
- cal->closed_loop_gain_const);
- correction = (int16_t)((product << 4) / (int32_t)normalizer);
+
+ int32_t product = MUL_S16(rt->angle_error_raw, cal->closed_loop_gain_const);
+ product = (int32_t)((uint32_t)product << 4);
+ correction = (normalizer != 0)
+ ? (int16_t)(product / (int32_t)normalizer)
+ : (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->angle_offset = shra16(rt->supervisor_state_17e, 4);
-}
-
-/* real_refine_submap (disassembled 8237–8257). 1D variant of combine. */
-static int16_t s_refine(const int16_t *y_base, const pwm_interp_slot_t *slot)
-{
- if (y_base == NULL || slot->x_interval == 0) return 0;
- const int16_t *yp = (const int16_t *)((const uint8_t *)y_base + slot->y_byte_off);
- int16_t y_here = *yp;
- int16_t y_prev = *(const int16_t *)((const uint8_t *)yp - 2);
- int32_t diff = MUL_S16((int16_t)(y_prev - y_here), slot->x_offset);
- return (int16_t)(y_here + (int32_t)(diff / (int32_t)slot->x_interval));
-}
-
-/* fast_recovery FUN_70ae (disassembled 70ae–713a). */
-static void s_recovery(pwm_runtime_t *rt,
- const pwm_calibration_t *cal,
- uint16_t rpm)
-{
- uint16_t mf = (uint16_t)rt->mode_flags;
- uint16_t sustained = (uint16_t)(((mf << 2) & mf));
- if (sustained > 0x30u) {
- if ((uint16_t)rt->recovery_counter
- >= (uint16_t)rt->recovery_count_threshold) {
- rt->system_flags_110 |= 0x01u;
- rt->recovery_counter = 0;
- rt->error_persist_counter = cal->error_persist_init_count;
- } else if ((int16_t)rpm > cal->recovery_rpm_threshold
- && (rt->system_flags_110 & 0x10u) == 0u
- && (rt->system_flags_110 & 0x20u) == 0u
- && rt->error_persist_counter == 0) {
- rt->recovery_counter = (int16_t)(rt->recovery_counter + 1);
- }
- } else if ((rt->system_flags_110 & 0x01u) == 0u) {
- rt->recovery_counter = 0;
- }
+ rt->angle_offset = shra16(rt->supervisor_state_17e, 4u);
}
/* ═════════════════════════════════════════════════════════════════════
- * Default calibration (TODO-placeholder values)
+ * Stage 3 — Publish CL (FUN_7cd8 @ 0x7cd8-0x7cea)
* ═════════════════════════════════════════════════════════════════════ */
-const pwm_calibration_t pwm_cal_default = {
- .target_minimum = 0,
- .y_pair = { NULL, NULL, NULL },
- .pi_upper_breakpoint = 107,
- .pi_lower_breakpoint = (int16_t)0xFF95,
- .pi_center_slope = 2560,
- .pi_upper_outer_slope = 5120,
- .pi_lower_outer_slope = 5120,
- .pi_upper_integrator_gain = 1024,
- .pi_center_integrator_gain = 1024,
- .pi_lower_integrator_gain = 2048,
- .setpoint_offset = 0, /* CAL+0x52 - CAL+0x54 — populated from ROM */
-};
-const pwm_flash_t pwm_flash_default = {
- .max_error_x = {0},
- .max_error_y = {0},
- .max_error_count = 1,
- .max_error_input_addr = 0,
- .request_upper_limit = 0,
- .large_pos_error_thresh = 0x200,
- .large_neg_error_thresh = (int16_t)0xFE00,
- .inj_qty_demand_thresh = 0,
- .rpm_breakpoints = {0},
- .rpm_values = {0},
- .shape_scale_src = 24,
- .period_max_limit = 0x8000,
- .period_min_limit = 0x6000,
- .pwm_y_table = NULL,
- .shape_y_table = NULL,
-};
+static void s_publish_cl(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal)
+{
+ s_cl_correct(rt, cal);
+ rt->estimated_angle = (int16_t)(rt->inputs.ckp_in + rt->angle_offset);
+}
-/* ROM-extracted pwm_cal_rom, pwm_flash_rom, pwm_submap_descrs[], and all
- * Y-table / submap-x static payloads live in cal_tables_rom.{h,c} — both
- * auto-generated by tools/extract_calibration.py --target compact. Link
- * cal_tables_rom.c alongside pwm.c to get them. */
+/* ═════════════════════════════════════════════════════════════════════
+ * Stage 4 — PI controller (FUN_67c4 @ 0x67c4 + FUN_66a8 + FUN_672b)
+ * ═════════════════════════════════════════════════════════════════════ */
+
+/* FUN_66a8 — saturation-latch recovery handler. Role-equivalent to the
+ * default family's `s_recovery` / `fast_recovery`; same name across
+ * variants for cross-family alignment. Disasm/source mapping (audit
+ * table — keep the cooldown gate and gated-increment branches in this
+ * exact order):
+ * 66ad-66b2 CMPB pi_shape_flag, pi_flag_c6 ; JNE LAB_671d
+ * 66b9-66be CMP pi_state_118, [0x02c0]=cal+0x112 ; JNC LAB_66e6
+ * JNC ⇒ counter < threshold ⇒ gated-increment
+ * fall-through ⇒ counter ≥ threshold ⇒ latch+reset
+ * 66c0-66e5 latch bit0 ; counter = 0 ; pi_state_c2 ← cal+0x114
+ * 66e6-66f4 CMP rpm, [cal+0x11E] ; JLE exit
+ * 66f6-6705 JBS bit4 / JBS bit5 — exit
+ * 6706-670b CMP ZR, pi_state_c2 ; JNE exit
+ * 670d-6717 pi_state_118 += 1
+ * 671d-6725 JBS bit0 exit ; pi_state_118 = 0
+ */
+static void s_recovery(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal)
+{
+ if (rt->pi_shape_flag != rt->pi_flag_c6) {
+ if ((rt->system_flags_110 & 0x01u) == 0u) {
+ rt->pi_state_118 = 0;
+ }
+ return;
+ }
+ /* Counter ≥ threshold → latch+reset. Threshold is boot-cached at
+ * RAM[0x02c0] from cal+0x112 by FUN_6b7b:0x6bd4. */
+ if ((uint16_t)rt->pi_state_118 >= (uint16_t)cal->pi_sat_count_threshold) {
+ rt->system_flags_110 = (uint8_t)(rt->system_flags_110 | 0x01u);
+ rt->pi_state_118 = 0;
+ rt->pi_state_c2 = cal->error_thresh_114;
+ return;
+ }
+ /* Gated increment: rpm > rpm_threshold_11E required. */
+ if ((int16_t)rt->inputs.rpm <= cal->rpm_threshold_11E) return;
+ /* Bits 4 or 5 of system_flags abort. */
+ if ((rt->system_flags_110 & 0x30u) != 0u) return;
+ /* c2 cooldown gate — counter advances only after c2 has decayed to 0. */
+ if (rt->pi_state_c2 != 0) return;
+ rt->pi_state_118 = (int16_t)(rt->pi_state_118 + 1);
+}
+
+/* FUN_7c85 @ 0x7c85-0x7cbf — PI integrator step. Anti-windup gates
+ * the update direction against pi_flag_c7 (0=in-range, 1=clamped low,
+ * 2=clamped high). Updates the {pi_b4_state:pi_b6_state} 32-bit pair.
+ * Disasm:
+ * 7c85 LD RW1C, err
+ * 7c8a MUL RL1C, [0x0456] ; signed 32-bit, RL1C = err*step
+ * 7c90 SHLL RL1C, #0x4 ; RL1C <<= 4
+ * 7c93 JGE LAB_7ca1 ; if signed result >= 0
+ * 7c95-7c9d if pi_flag_c7==1 RET ; (clamped-low → don't push more negative)
+ * 7c9f SJMP LAB_7cab ; else update
+ * LAB_7ca1: if pi_flag_c7==2 RET ; (clamped-high → don't push more positive)
+ * LAB_7cab: integrator += RL1C ; ADD lo, ADDC hi
+ */
+static void s_pi_integrator_step(pwm_runtime_t *rt)
+{
+ int32_t prod = MUL_S16(rt->angle_error_pi, rt->pi_integ_step_456);
+ int32_t step = (int32_t)((uint32_t)prod << 4);
+
+ if (step >= 0) {
+ if (rt->pi_flag_c7 == 0x02) return; /* anti-windup: clamped-high */
+ } else {
+ if (rt->pi_flag_c7 == 0x01) return; /* anti-windup: clamped-low */
+ }
+
+ /* Combine {hi:lo} as int32 -> add step -> split back. */
+ int32_t accum = ((int32_t)(int16_t)rt->pi_b4_state << 16)
+ | (uint16_t)rt->pi_b6_state;
+ accum += step;
+ rt->pi_b6_state = (int16_t)(accum & 0xFFFF);
+ rt->pi_b4_state = (int16_t)((accum >> 16) & 0xFFFF);
+}
+
+/* FUN_672b @ 0x672b-0x67c3 — PI compensation + final clamp.
+ * Two arms decided by (pi_flag_338==1 || pi_open_loop_flag==0):
+ * "fresh" → reset pi_b4_state from a new (err*pi_gain>>4)+ckp formula
+ * "settled" → step the integrator via FUN_7c85
+ * Then compute compensation_angle = (err * pi_post_scale_454) >> 8
+ * and active_request = comp + pi_b4_state, clamped to [pi_low_clamp, target].
+ */
+static void s_pi_compensation(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal)
+{
+ int16_t error = rt->angle_error_pi;
+
+ /* [0x672b-0x673a] Branch select. */
+ bool fresh = (rt->pi_flag_338 == 0x01) || (rt->pi_open_loop_flag == 0u);
+
+ if (fresh) {
+ /* [LAB_673c 0x673c-0x6762] Reset pi_b4_state. Disasm:
+ * STB ZRlo, DAT_0338 ; pi_flag_338 = 0
+ * LD RW1C, err
+ * EXT RL1C ; sign-extend to 32-bit
+ * MUL RL1C, [0x0330] ; signed: RL1C = err32 * pi_gain
+ * SHRA RW1C, #0x4 ; LOW WORD shifted (high word discarded)
+ * ADD RW1C, RW1C, ckp_in
+ * ST RW1C, DAT_02b4 ; pi_b4_state = result
+ */
+ rt->pi_flag_338 = 0;
+ int32_t prod = MUL_S16(error, (int16_t)rt->pi_integ_gain_330);
+ int16_t scaled = shra16((int16_t)(prod & 0xFFFF), 4);
+ rt->pi_b4_state = (int16_t)(scaled + rt->inputs.ckp_in);
+ } else {
+ /* [LAB_6764 0x6764] LCALL FUN_7c85 — integrator step. */
+ s_pi_integrator_step(rt);
+ }
+
+ /* [LAB_6767 0x6767-0x6779] compensation_angle = (RL1C = post_scale*err) >> 8.
+ * Disasm:
+ * LD RW1C, [0x0454] ; post_scale (default +480)
+ * MUL RL1C, err ; RL1C = post_scale * err (signed)
+ * SHRAL RL1C, #0x8 ; arithmetic shift right 8 (32-bit)
+ * ST RW1C, DAT_02b8 ; compensation_angle = lo16
+ */
+ int32_t comp32 = MUL_S16(rt->pi_post_scale_454, error);
+ int16_t comp = (int16_t)(shra32(comp32, 8) & 0xFFFF);
+ rt->compensation_angle = comp;
+
+ /* [0x677a-0x677f] active_request = comp + pi_b4_state */
+ int16_t combined = (int16_t)(comp + rt->pi_b4_state);
+ rt->active_request = combined;
+
+ /* [0x6782-0x67a8] Lower clamp: if active_request < pi_low_clamp,
+ * pin to pi_low_clamp and set pi_flag_c7=1. */
+ if (rt->active_request < cal->pi_low_clamp) {
+ rt->active_request = cal->pi_low_clamp;
+ rt->pi_flag_c7 = 0x01;
+ return;
+ }
+
+ /* [LAB_67a9 0x67a9-0x67bd] Upper clamp: if active_request > target,
+ * pin to target and set pi_flag_c7=2. */
+ /* Disasm 0x67a9-0x67b0: CMP RW46, DAT_0336 — upper clamp uses
+ * the RPM-derived ceiling, not the CAN-decoded primary setpoint. */
+ if (rt->active_request > rt->target_336) {
+ rt->active_request = rt->target_336;
+ rt->pi_flag_c7 = 0x02;
+ return;
+ }
+
+ /* [LAB_67be 0x67be-0x67c3] In-range: pi_flag_c7 = 0. */
+ rt->pi_flag_c7 = 0;
+}
+
+static void s_pi_update(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal)
+{
+ /* PI CL-gate threshold = cached ROM[0x605c]. Disasm site:
+ * FUN_67c4:67d9 `CMP RW1C, DAT_605c` (literal 0x01A4 = 420). In the
+ * real ECU, state_130 is almost always negative (CAN inactive
+ * sentinel) which forces OL at line 67d4 before this RPM gate is
+ * reached. Lifted to cal->pi_cl_rpm_floor. */
+ int16_t rpm_floor = cal->pi_cl_rpm_floor;
+ bool open_loop = false;
+ if ((rt->system_flags_110 & 0x20u) != 0u) open_loop = true;
+ else if (rt->inputs.state_130 < 0) open_loop = true;
+ else if ((int16_t)rt->inputs.rpm < rpm_floor) open_loop = true;
+
+ if (open_loop) {
+ rt->pi_open_loop_flag = 0;
+ /* Disasm 0x67e5: LD RW46, RW5E — open-loop assigns the CAN-decoded
+ * primary setpoint, NOT the RPM-derived ceiling. */
+ rt->active_request = rt->target_5e;
+ /* cal+0x120 is a LOWER bound here (disasm: JLT clamps when
+ * active_request < limit). Same constant also serves as the
+ * Block-4 low clamp, hence `pi_low_clamp`. See
+ * families/t06211/src/pi_controller_t06211.c for the full-disasm
+ * annotation. */
+ if (rt->active_request < cal->pi_low_clamp) {
+ rt->active_request = cal->pi_low_clamp;
+ }
+ goto final_flag;
+ }
+
+ /* Disasm 0x680a: SUB RW1C, RW5E, DAT_02cc — CL error uses target. */
+ rt->angle_error_pi = (int16_t)(rt->target_5e - rt->estimated_angle);
+ int16_t err = rt->angle_error_pi;
+
+ if (err > cal->large_pos_error_thresh) {
+ rt->pi_shape_flag = 0;
+ s_recovery(rt, cal);
+ } else if (err < cal->large_neg_error_thresh) {
+ rt->pi_shape_flag = 0x01;
+ if (rt->inputs.inj_qty_demand <= cal->pi_thresh_116) {
+ rt->pi_state_118 = 0;
+ } else {
+ s_recovery(rt, cal);
+ }
+ } else {
+ rt->pi_shape_flag = 0x20;
+ if (rt->pi_state_c2 > 0) {
+ rt->pi_state_c2 = (int16_t)(rt->pi_state_c2 - 1);
+ }
+ if (rt->pi_state_c2 == 0) {
+ rt->system_flags_110 = (uint8_t)(rt->system_flags_110 & 0xFEu);
+ }
+ if (rt->pi_state_118 > 0) {
+ rt->pi_state_118 = (int16_t)(rt->pi_state_118 - 1);
+ }
+ }
+
+ /* ── Block 4: error-window decision [0x68b7-0x68ef] ──
+ * Strict > / < per disasm JLE 68c9 / JGE 68ed. Bounds at
+ * 0x0450/0x0452 are set once at boot by FUN_76aa — see
+ * docs/open-questions.md §2 closeout. */
+ if (err > rt->pi_error_bound_pos) {
+ rt->active_request = cal->pi_high_clamp;
+ rt->pi_flag_338 = 0x01;
+ } else if (err < rt->pi_error_bound_neg) {
+ rt->active_request = cal->pi_low_clamp;
+ rt->pi_flag_338 = 0x01;
+ } else {
+ s_pi_compensation(rt, cal);
+ }
+
+final_flag:
+ rt->pi_open_loop_flag = 0x01;
+ rt->pi_flag_c6 = rt->pi_shape_flag;
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Stage 5 — PWM output (FUN_5314 @ 0x5314-0x565f)
+ * ═════════════════════════════════════════════════════════════════════ */
+
+static void s_pwm_output(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal)
+{
+ /* A. eval pwm_A */
+ s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_PWM_A],
+ &rt->pwm_slot_a);
+
+ /* B. eval pwm_B */
+ s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_PWM_B],
+ &rt->pwm_slot_b);
+
+ /* C. combine bilinear */
+ int16_t duty = s_combine(cal->pwm_y_table, &rt->pwm_slot_a, &rt->pwm_slot_b);
+ rt->pwm_duty = (uint16_t)duty;
+
+ /* D. Duty-range classification [0x5361-0x538b] — based on pre-shape duty. */
+ if (rt->pwm_duty < 0x29u) rt->pwm_duty_range_flag = 1u;
+ else if (rt->pwm_duty > 0xFD7u) rt->pwm_duty_range_flag = 2u;
+ else rt->pwm_duty_range_flag = 0u;
+
+ /* E1. RPM-window three-phase matcher [0x53be-0x552a].
+ *
+ * Phase 1 (strict-band): rpm strictly inside any of 4 bands at
+ * cal+0xF2 → slew_increment = +pwm_slew_step.
+ * Phase 2 (hysteresis margin): rpm in any band's halfwidth-extended
+ * region → HOLD (preserve previous slew_increment).
+ * Phase 3 (deep-out): rpm clear of every extended band AND
+ * pwm_period < pwm_period_max → slew_increment = -pwm_slew_step.
+ *
+ * Slew applied as `pwm_period -= slew_increment` (subtraction;
+ * positive increment shrinks period → faster PWM).
+ * pwm_slew_increment lives in rt and persists across cycles so the
+ * HOLD path keeps the previous direction.
+ *
+ * Disasm note: high-edge tests use JC (db) at 0x5463/0x5497 for
+ * bands 0/1 and JNC (d3) at 0x54cb/0x54fd for bands 2/3 — these
+ * encodings produce equivalent control flow (HOLD on rpm < hi+hw,
+ * advance on rpm >= hi+hw). See open-questions.md §5 closeout. */
+ uint16_t *pwm_period = (uint16_t *)&rt->pwm_shape_state[0];
+ const int16_t rpm_s = (int16_t)rt->inputs.rpm;
+ const int16_t halfwidth = cal->pwm_window_halfwidth;
+ const int16_t step_mag = cal->pwm_slew_step;
+
+ /* Phase 1 — strict-band test [0x53be-0x542d] */
+ int slew_set = 0;
+ for (int bi = 0; bi < 4; bi++) {
+ int16_t lo = cal->pwm_rpm_windows[bi * 2];
+ int16_t hi = cal->pwm_rpm_windows[bi * 2 + 1];
+ if (rpm_s > lo && rpm_s < hi) {
+ rt->pwm_slew_increment = step_mag;
+ slew_set = 1;
+ break;
+ }
+ }
+
+ if (!slew_set) {
+ /* Phase 2 — fine hysteresis margin [0x5434-0x54fe] */
+ int phase3 = 1;
+ for (int bi = 0; bi < 4; bi++) {
+ int16_t lo = cal->pwm_rpm_windows[bi * 2];
+ int16_t hi = cal->pwm_rpm_windows[bi * 2 + 1];
+ if (rpm_s <= (int16_t)(lo - halfwidth)) {
+ continue; /* below extended low: try next band */
+ }
+ if (rpm_s < (int16_t)(hi + halfwidth)) {
+ phase3 = 0; /* HOLD */
+ break;
+ }
+ /* rpm_s >= hi + halfwidth: advance to next band */
+ }
+ if (phase3 && *pwm_period < cal->pwm_period_max) {
+ /* Phase 3 — deep-out negative slew [0x54ff-0x551b] */
+ rt->pwm_slew_increment = (int16_t)(-step_mag);
+ }
+ /* else: HOLD (no write) */
+ }
+
+ /* Slew application [0x5520-0x552a] */
+ int32_t period_next = (int32_t)*pwm_period - (int32_t)rt->pwm_slew_increment;
+ if (period_next < 0) period_next = 0;
+ if (period_next > 0xFFFF) period_next = 0xFFFF;
+ *pwm_period = (uint16_t)period_next;
+
+ /* Final clamp [0x552f-0x5575] */
+ if (*pwm_period < cal->pwm_period_min) *pwm_period = cal->pwm_period_min;
+ if (*pwm_period > cal->pwm_period_max) *pwm_period = cal->pwm_period_max;
+
+ /* E2. Shape detail [0x557a-0x5608] */
+ pwm_interp_slot_t shape_slot;
+ s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_SHAPE_EVAL],
+ &shape_slot);
+ int16_t shape_height = s_refine(cal->shape_y_table, &shape_slot);
+ if (shape_height < 0) shape_height = 0;
+ if (shape_height > 0x199) shape_height = 0x199;
+ rt->pwm_shape_state[5] = shape_height;
+
+ /* E3. Shape composition additive [0x55cb-0x560f]. ROM formula:
+ * slope = (period_max - period_min) >> 8
+ * numerator = ((period_max - pwm_period) >> 8) * shape_height
+ * pwm_duty += numerator / slope
+ * Shifts happen before multiply (MCS-96 16-bit intermediate). */
+ int16_t slope = (int16_t)(((int32_t)cal->pwm_period_max
+ - (int32_t)cal->pwm_period_min) >> 8);
+ int16_t pmx_delta = (int16_t)(((int32_t)cal->pwm_period_max
+ - (int32_t)*pwm_period) >> 8);
+ int32_t shape_add = slope ? (MUL_S16(pmx_delta, shape_height)
+ / (int32_t)slope)
+ : 0;
+ int32_t duty_new = (int32_t)rt->pwm_duty + shape_add;
+
+ /* F. Duty bounds clamp [0x5614-0x5636] — ROM reads RAM[0x6058]/[0x605a]
+ * (producers open-questions §3). Lifted to cal->pwm_min/cal->pwm_max
+ * (defaults 205/3890, same as the default family). */
+ if (duty_new < (int32_t)cal->pwm_min) duty_new = (int32_t)cal->pwm_min;
+ if (duty_new > (int32_t)cal->pwm_max) duty_new = (int32_t)cal->pwm_max;
+ rt->pwm_duty = (uint16_t)duty_new;
+
+ /* G. HW shadow writes [0x563b-0x565f] using the slewed pwm_period. */
+ uint32_t on_product = (uint32_t)(*pwm_period) * (uint32_t)rt->pwm_duty;
+ rt->pwm_on_time = (uint16_t)(on_product / 0xFFFu);
+ rt->pwm_off_time = (uint16_t)(*pwm_period - rt->pwm_on_time);
+ rt->pwm_period = *pwm_period;
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Bypass-PI LUT helper: query the ROM Y-table directly with (rpm, fbkw)
+ * as the two axes, applying the same eval+combine+clamp the PWM stage
+ * does. Useful for plotting the static (rpm, fbkw) → duty surface
+ * without the PI controller in the loop.
+ * ═════════════════════════════════════════════════════════════════════ */
+
+uint16_t pwm_lut_duty(const pwm_calibration_t *cal,
+ uint16_t rpm, int16_t fbkw)
+{
+ pwm_submap_descr_t a = pwm_submap_descrs[PWM_SUBMAP_PWM_A];
+ pwm_submap_descr_t b = pwm_submap_descrs[PWM_SUBMAP_PWM_B];
+ int16_t rpm_signed = (int16_t)rpm;
+ a.input_ptr = &rpm_signed;
+ b.input_ptr = &fbkw;
+ pwm_interp_slot_t sa, sb;
+ s_eval_submap(&a, &sa);
+ s_eval_submap(&b, &sb);
+ int16_t duty = s_combine(cal->pwm_y_table, &sa, &sb);
+ if (duty < 205) duty = 205;
+ if (duty > 3890) duty = 3890;
+ return (uint16_t)duty;
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Public entry — pwm_service
+ * Mirrors FUN_77b3 (0x77b3-0x77d8) — 5-call linear dispatcher.
+ * ═════════════════════════════════════════════════════════════════════ */
+
+void pwm_service(pwm_runtime_t *rt)
+{
+ read_inputs(rt);
+
+ const pwm_calibration_t *cal = rt->bound_cal;
+
+ s_setpoint(rt); /* [0x77b3-0x77c7] target_336 = pwm_A interp(rpm) */
+ s_setpoint_can_decode(rt, cal); /* CAN-decoded target (FUN_64c3 in ROM) */
+ s_supervisor(rt); /* [0x77cc] */
+ s_publish_cl(rt, cal);/* [0x77cf] */
+ s_pi_update(rt, cal); /* [0x77d2] */
+ s_pwm_output(rt, cal);/* [0x77d5] */
+}
diff --git a/Core/Advance_Control/pwm.h b/Core/Advance_Control/pwm.h
index 0cc2d6f..6981cb8 100644
--- a/Core/Advance_Control/pwm.h
+++ b/Core/Advance_Control/pwm.h
@@ -1,19 +1,21 @@
/**
- * @file pwm.h
- * @brief Compact single-header API for the VP44 PWM solenoid controller.
+ * @file pwm.h (families/t06211/compact_src)
+ * @brief Compact single-header API for the t06211 PWM controller.
+ *
+ * Mirrors the public shape of the default family's `compact_src/pwm.h`
+ * but tailored to t06211's 5-stage pipeline (no standalone max_cl_error
+ * lookup, single-submap setpoint, factor+offset target compute).
*
* Public API is just two functions:
- * pwm_init() — one-time setup; caches cal/flash/getters into rt
- * pwm_service() — per-cycle update; pulls external inputs via getters,
- * runs the 6-stage control pipeline, writes rt outputs
+ * pwm_init() — one-time setup
+ * pwm_service() — per-cycle update (pulls inputs via getters,
+ * runs the 5-stage pipeline, writes rt outputs)
*
- * External inputs (sensors, CAN, HW) arrive exclusively through the
- * pwm_input_getters_t vtable. Each getter returns a value in native PWM
- * units — put any unit/scale conversion inside your getter body.
+ * External inputs arrive through a getter vtable; each callback returns
+ * one signal in native PWM units.
*
- * Every arithmetic choice (MUL vs MULU, SHRA vs SHR, JGE vs JC) mirrors the
- * MCS-96 assembly; see src/ for per-stage commentary + disassembly address
- * cross-references.
+ * Every arithmetic choice mirrors the MCS-96 assembly; see
+ * families/t06211/src/ for per-stage commentary.
*/
#ifndef PWM_H
#define PWM_H
@@ -26,10 +28,7 @@
#define MUL_S16(a, b) ((int32_t)(int16_t)(a) * (int32_t)(int16_t)(b))
#define MULU_U16(a, b) ((uint32_t)(uint16_t)(a) * (uint32_t)(uint16_t)(b))
#define CLAMP(v, lo, hi) ((v) < (lo) ? (lo) : (v) > (hi) ? (hi) : (v))
-#define INTEG_HI(s) ((int16_t)(((s) >> 16) & 0xFFFF))
-#define INTEG_LO(s) ((uint16_t)((s) & 0xFFFF))
-/** Portable signed right shift — guarantees sign extension (SHRA/SHRAL). */
static inline int16_t shra16(int16_t v, unsigned n) {
if (!n) return v;
uint16_t u = (uint16_t)v, sign = (u >> 15) & 1u;
@@ -43,220 +42,274 @@ static inline int32_t shra32(int32_t v, unsigned n) {
return (int32_t)((u >> n) | m);
}
-/* ── Mode flag bit definitions (address 0x028c) ─────────────────────── */
-#define MODE_FIRST_CALL 0x01 /* open→closed transition */
-#define MODE_OUTER_REGION 0x02 /* error outside center region */
-#define MODE_NEG_SAT 0x04 /* lower saturation */
-#define MODE_POS_SAT 0x08 /* upper saturation */
-#define MODE_LARGE_POS 0x10 /* large positive error */
-#define MODE_LARGE_NEG 0x20 /* large negative error */
-#define MODE_PREV_MASK 0xC0 /* shadow of prev cycle bits 4-5 */
-
-#define INTERP_MAX_ENTRIES 16
-
/* ══════════════════════════════════════════════════════════════════════
* 1D INTERPOLATION SLOT
- * Decoded by eval_submap once per input, then consumed by combine_submaps
- * (bilinear, takes two slots) or refine_submap (1D, takes one slot).
- * Four int16s; no cycle-to-cycle state.
+ * Layout matches family-1's pwm_interp_slot_t and the family-1 ROM
+ * convention [count*2, range, offset, seg_bytes] — t06211's FUN_6fb8
+ * fingerprints identically, so the same layout applies.
* ══════════════════════════════════════════════════════════════════════ */
typedef struct pwm_interp_slot {
- int16_t row_stride; /* count * 2 — Y-table row byte-stride (A-axis width) */
- int16_t x_interval; /* x[k-1] - x[k] — denominator (dx between breaks) */
- int16_t x_offset; /* input - x[k] — numerator (distance from lower break) */
- int16_t y_byte_off; /* k * 2 — Y-table byte-offset at break k */
+ int16_t row_stride; /* count * 2 (byte stride for Y-table rows) */
+ int16_t x_interval; /* x[k-1] - x[k] */
+ int16_t x_offset; /* input - x[k] */
+ int16_t y_byte_off; /* k * 2 */
} pwm_interp_slot_t;
/* ══════════════════════════════════════════════════════════════════════
* EXTERNAL INPUTS
- * Populated each cycle by pwm_service via the getter vtable. All values
- * must be in native PWM units — do any unit/scale conversion inside your
- * getter. Address comments are the MCS-96 RAM location of the original
- * variable in the ROM (for cross-reference with disassembly only).
+ * Populated each cycle by pwm_service via the getter vtable.
+ * t06211-specific set: no temperature, no angle_dec_cmd in the setpoint
+ * (the family-2 setpoint is single-submap RPM-indexed).
* ══════════════════════════════════════════════════════════════════════ */
typedef struct pwm_inputs {
- int16_t ckp_in; /* 0x02f8 — CKP-derived sensed position */
- uint16_t rpm; /* 0x0040 — engine RPM */
- int16_t angle_dec_cmd; /* 0x0042 — CAN: angle-decrease command (sp2_B submap input) */
- int16_t inj_qty_demand; /* 0x0044 — CAN: injection quantity demand (sp0_B + PI large-neg threshold) */
- int16_t b_fb_kw; /* 0x02b4 — CAN: B_FB_KW plunger feedback demand (setpoint baseline) */
- int16_t state_130; /* 0x0130 — CAN: open/closed-loop discriminant (< 0 forces open-loop) */
- uint16_t supply_voltage; /* 0x0142 — battery / supply voltage (shape_eval submap input) */
- int16_t temperature; /* 0x0146 — temperature (sp1_B submap input) */
- /* NOTE: pwm_B's submap input_var=0x0046 in the ROM. That address is RW46,
- * the register into which finalize_angle_request writes active_request
- * at exit (disasm 0x73f4). It is NOT an external input; our port binds
- * that descriptor's input_ptr directly to rt->active_request. */
+ int16_t ckp_in; /* 0x02f8 — sensed position */
+ uint16_t rpm; /* 0x0040 — engine RPM */
+ int16_t angle_dec_cmd; /* 0x0042 — accepted for family-1 API parity; unused here */
+ int16_t inj_qty_demand; /* 0x0044 — CAN: inj quantity demand (PI large-neg gate) */
+ int16_t b_fb_kw; /* CAN: plunger feedback baseline. Gets copied into
+ can_raw_b_fb_kw and decoded by s_setpoint_can_decode
+ (port of FUN_64c3) to produce target. */
+ int16_t state_130; /* 0x0130 — CAN: open/closed-loop discriminant */
+ uint16_t supply_voltage; /* 0x0142 — shape_eval submap input */
+ int16_t temperature; /* 0x0146 — accepted for family-1 API parity; unused here */
} pwm_inputs_t;
-/** Getter vtable. Each callback reads one external signal from your system
- * and returns it in native PWM units. `ctx` is opaque to the PWM library
- * and is passed back unchanged so your getters can reach their own state.
- * All callbacks are required (no NULL entries).
- */
+/** Getter vtable. 8-callback layout matches family-1's pwm_input_getters_t
+ * so a single FBKW.c can drive either family's pwm.c without changes.
+ * t06211 does not consume angle_dec_cmd or temperature; those getters are
+ * called and stored into rt->inputs but the pipeline ignores them. */
typedef struct pwm_input_getters {
int16_t (*ckp_in) (void *ctx);
uint16_t (*rpm) (void *ctx);
- int16_t (*angle_dec_cmd) (void *ctx);
+ int16_t (*angle_dec_cmd) (void *ctx); /* accepted; unused by t06211 */
int16_t (*inj_qty_demand)(void *ctx);
int16_t (*b_fb_kw) (void *ctx);
int16_t (*state_130) (void *ctx);
uint16_t (*supply_voltage)(void *ctx);
- int16_t (*temperature) (void *ctx);
- void *ctx; /* user data; forwarded to every getter */
+ int16_t (*temperature) (void *ctx); /* accepted; unused by t06211 */
+ void *ctx;
} pwm_input_getters_t;
-/* Forward decls for the vtables */
typedef struct pwm_calibration pwm_calibration_t;
-typedef struct pwm_flash pwm_flash_t;
typedef struct pwm_submap_descr pwm_submap_descr_t;
/* ══════════════════════════════════════════════════════════════════════
* RUNTIME STATE
- * Holds everything that persists cycle-to-cycle plus per-cycle working
- * values. External inputs live in rt->inputs (populated each cycle).
+ * RAM-address comments reference the t06211 MCS-96 map (see
+ * families/t06211/docs/variable-glossary.md).
* ══════════════════════════════════════════════════════════════════════ */
typedef struct pwm_runtime {
- /* External inputs — refreshed each cycle by pwm_service from getters */
+ /* External inputs — refreshed each cycle from getters */
pwm_inputs_t inputs;
- /* Async / mixed-direction flags.
- * reset_flag — set by user/ISR on reset edge; cleared by supervisor.
- * system_flags_110 — bit 5 (0x20): external "force open-loop" request;
- * bit 0 (0x01): internal latch set by fast_recovery,
- * cleared by PI when error_persist_counter hits zero.
+ /* Async flags.
+ * reset_flag — set on reset edge; supervisor clears.
+ * system_flags_110 — bit 5 forces open-loop; bit 0 is fast-recovery latch.
*/
- uint8_t reset_flag; /* 0x002e */
- uint8_t system_flags_110; /* 0x0110 */
+ uint8_t reset_flag; /* 0x002e */
+ uint8_t system_flags_110; /* 0x0110 */
- /* ── Setpoint pipeline ───────────────────────────────────────────── */
- int16_t compensation_angle; /* 0x02b6 — sum of 3 submap-pair outputs */
- int16_t pre_offset_target; /* 0x012a — (b_fb_kw + compensation_angle) >> 1 */
- int16_t setpoint_offset; /* 0x0150 — user-supplied setpoint bias */
- int16_t target; /* RW5E — final setpoint (clamped) */
- pwm_interp_slot_t setpoint_slot_x; /* 0x02b8 — per-call eval → combine */
- pwm_interp_slot_t setpoint_slot_y; /* 0x02c0 */
+ /* ── Dual-target setpoint architecture ── */
+ /* target (RW5E = RAM[0x005e]): PRIMARY CAN-driven setpoint
+ * computed by FUN_64c3 (called from CAN parser FUN_6192). Drives
+ * the PI controller in both open-loop (FUN_67c4:67e5) and
+ * closed-loop (FUN_67c4:680a) paths, and supervisor error compute
+ * (FUN_7beb:7c20). Formula: raw/2 + setpoint_offset_150 + rw42_state,
+ * clamped >= cal->target_5e_min_clamp. */
+ int16_t target_5e; /* 0x005e — primary CAN-decoded setpoint */
+ /* target_336 (DAT_0336): SECONDARY RPM-derived ceiling computed by
+ * FUN_7168(setpoint_descr) + FUN_77b3:77c7. Used ONLY as upper-clamp
+ * in FUN_672b PI compensation (0x67a9/0x67b0) and FUN_7beb supervisor
+ * (0x7c30/0x7c37). Does not drive control directly. */
+ int16_t target_336; /* 0x0336 — RPM-derived ceiling */
+ /* CAN-staging buffers — written externally before pwm_service so
+ * s_setpoint_can_decode can transform them into target. */
+ int16_t can_raw_b_fb_kw; /* 0x012c — raw b_fb_kw from CAN */
+ int16_t can_aux_12e; /* 0x012e — secondary CAN field (drives rw42_state) */
+ int16_t can_half_12a; /* 0x012a — cached raw>>1 (debug visibility) */
+ /* setpoint_offset_150 (RAM[0x0150]) is copied at runtime_init from
+ * cal.setpoint_offset (mirrors family-1's pattern at
+ * compact_src/pwm.c:66). The REAL ROM derives it at boot via
+ * FUN_6b7b @ 0x6bb5: RAM[0x150] = cal+0x4c - RW1E. Because RW1E's
+ * source at the call site (0x6c46) is in a Ghidra-unrecognised code
+ * region, we expose the field directly through cal so the user can
+ * set it to the match-fitted value without needing to simulate the
+ * full FUN_6b7b boot path. */
+ int16_t setpoint_offset_150; /* 0x0150 */
+ int16_t rw42_state; /* RW42 register — = can_aux_12e when valid */
+ int16_t b_fb_kw_baseline; /* 0x033a — latched snapshot of b_fb_kw on reset */
+ int16_t compensation_angle; /* 0x02b8 — PI-helper output consumed in supervisor */
+ int16_t angle_error_raw; /* 0x033e — supervisor output (raw + ceiling-clamped) */
+ uint16_t cl_enable_counter; /* 0x033c */
- /* ── Supervisor ──────────────────────────────────────────────────── */
- int16_t cl_enable_counter; /* 0x0340 */
- int16_t cl_error_delta; /* 0x0342 */
- int16_t max_cl_error; /* 0x02f2 */
- int16_t supervisor_state_17e; /* 0x017e — CL-correction accumulator */
+ /* ── CL correction (RAM-layout identical to family 1) ── */
+ int16_t cl_correction_raw; /* 0x0176 */
+ int16_t angle_offset; /* 0x017c */
+ int16_t supervisor_state_17e; /* 0x017e — CL accumulator */
+ int16_t pos_error_normalizer; /* 0x0332 */
+ int16_t neg_error_normalizer; /* 0x0334 */
- /* ── PI controller ───────────────────────────────────────────────── */
- int16_t angle_error; /* 0x0278 */
- int16_t angle_error_shaped; /* 0x0276 */
- uint8_t mode_flags; /* 0x028c */
- int32_t integrator_state; /* 0x0288/028a */
- int16_t integrator_gain; /* 0x0286 */
- int16_t active_request; /* 0x0274 — final angle request */
- int16_t estimated_angle; /* 0x02cc */
- int16_t angle_offset; /* 0x017c */
- int16_t cl_correction_raw; /* 0x0176 */
- int16_t recovery_counter; /* 0x0118 */
- int16_t recovery_count_threshold;/* 0x027a */
- int16_t error_persist_counter; /* 0x027c */
- int16_t first_call_gain; /* 0x02ec */
- int16_t pos_error_normalizer; /* 0x02ee */
- int16_t neg_error_normalizer; /* 0x02f0 */
- int16_t min_rpm_openloop; /* 0x015c */
+ /* ── Publish + PI ── */
+ int16_t estimated_angle; /* 0x02cc */
+ int16_t angle_error_pi; /* 0x02be */
+ int16_t active_request; /* 0x0046 (RW46) — PI output / PWM feed-forward */
+ uint8_t pi_open_loop_flag; /* 0x02c4 */
+ uint8_t pi_shape_flag; /* 0x02c5 */
+ uint8_t pi_flag_c6; /* 0x02c6 */
+ uint8_t pi_flag_338; /* 0x0338 */
+ /* PI Block-4 error-window bounds (independent int16s, NOT a 32-bit
+ * integrator). Boot-initialised once by FUN_76aa @ 0x76aa — see
+ * docs/open-questions.md §2 (resolved). Trim bytes at 0x0414/0x0416
+ * have no writers in the ROM, so bounds default to ±853 at runtime. */
+ int16_t pi_error_bound_pos; /* 0x0450 — default +853 */
+ int16_t pi_error_bound_neg; /* 0x0452 — default -853 */
+ int16_t pi_state_118; /* 0x0118 */
+ int16_t pi_state_c2; /* 0x02c2 */
- /* PI shaping (populated from CAL by pwm_init) */
- int16_t upper_breakpoint, lower_breakpoint;
- int16_t center_slope, upper_outer_slope, lower_outer_slope;
- int16_t upper_integrator_gain, center_integrator_gain, lower_integrator_gain;
+ /* PI integrator at {RAM[0x02b4]:RAM[0x02b6]} — was misnamed "b_fb_kw"
+ * across the original glossary. Disasm `disasm_nav rw 0x02b4` shows
+ * only internal writers (FUN_672b, FUN_76aa, FUN_7c85), confirming
+ * this is purely a PI integrator state, not an external/CAN input.
+ * Treated as a signed 32-bit pair: hi at 0x02b4 (the value used in
+ * active_request = comp + pi_b4_state), lo at 0x02b6. */
+ int16_t pi_b4_state; /* 0x02b4 — integrator high word */
+ int16_t pi_b6_state; /* 0x02b6 — integrator low word */
- /* ── PWM output ──────────────────────────────────────────────────── */
- uint16_t pwm_duty; /* 0x02d2 */
- uint16_t pwm_period; /* 0x0330 */
- uint16_t pwm_on_time, pwm_off_time; /* 0x02ce / 0x02d0 — HW compare regs */
- uint8_t pwm_status_flag; /* 0x00d1 */
- int16_t shape_scale; /* 0x0338 */
- int16_t shape_intermediate; /* 0x0332 */
- int16_t shape_output; /* 0x033a */
- uint16_t pwm_min, pwm_max; /* 0x0158 / 0x015a */
- pwm_interp_slot_t pwm_slot_x; /* 0x0290 — pwm_A eval → combine */
- pwm_interp_slot_t pwm_slot_y; /* 0x0298 — pwm_B eval → combine */
+ /* PI compensation scalars — boot-set by FUN_76aa from cal+0x118/0x11A
+ * with optional <<4 byte trims at RAM[0x0410]/[0x0412] (no writers,
+ * default 0). Defaults: 0x0454=+480 (post-scale), 0x0456=+256 (step). */
+ int16_t pi_post_scale_454; /* 0x0454 — multiplier in comp = (err*scale)>>8 */
+ int16_t pi_integ_step_456; /* 0x0456 — multiplier in FUN_7c85 integrator step */
- /* ── Bindings (set once by pwm_init, consumed by pwm_service) ───── */
+ /* PI integ gain — boot-set by FUN_76aa from cal+0x11C (byte, clamped
+ * ≤15). Used in the "reset" branch as: pi_b4_state = (err*gain)>>4 + ckp.
+ * The runtime field formerly named "pwm_period" was a holdover from
+ * family 1 where 0x0330 had a different semantic; in t06211, 0x0330
+ * is this PI gain. */
+ uint8_t pi_integ_gain_330; /* 0x0330 — boot default 6 (cal+0x11C) */
+
+ /* Anti-windup flag set by FUN_672b clamps; consulted by FUN_7c85
+ * to gate integration direction. Values: 0 (in-range), 1 (clamped
+ * low), 2 (clamped high). */
+ uint8_t pi_flag_c7; /* 0x02c7 */
+
+ /* ── PWM output ── */
+ uint16_t pwm_duty; /* 0x02d2 */
+ uint16_t pwm_on_time; /* 0x02ce */
+ uint16_t pwm_off_time; /* 0x02d0 */
+ /* pwm_period — total period in Timer2 ticks = on_time + off_time.
+ * Mirrored from t06211 RAM[0x02e4] (computed each cycle by the PWM
+ * output stage). Exposed for family-1 FBKW.c API parity
+ * (UpdatePWM(&htim4, ch, pwm_on_time, pwm_period)). */
+ uint16_t pwm_period;
+ uint8_t pwm_duty_range_flag; /* 0x00d1 */
+ pwm_interp_slot_t pwm_slot_a; /* 0x02d4 */
+ pwm_interp_slot_t pwm_slot_b; /* 0x02dc */
+ /* pwm_shape_state[6] mirrors RAM[0x02e4..0x02ee]:
+ * [0] pwm_period working value (RAM 0x02e4)
+ * [1] (unused — was slew_increment, broken out as field below)
+ * [2] (RAM 0x02e8 — ROM band-array ptr, unused in C)
+ * [3] (RAM 0x02ea — ROM halfwidth ptr, unused in C)
+ * [4] (RAM 0x02ec — slew step magnitude, unused; read from cal)
+ * [5] shape_height (RAM 0x02ee, E2 output) */
+ int16_t pwm_shape_state[6]; /* 0x02e4-0x02ee */
+ /* Persistent slew_increment (RAM[0x02e6]). Carried across cycles so
+ * the hysteresis-margin HOLD path can preserve previous direction.
+ * Subtracted from pwm_period each cycle. See open-questions §5. */
+ int16_t pwm_slew_increment; /* 0x02e6 */
+
+ /* ── Bindings (set once by pwm_init) ── */
const pwm_calibration_t *bound_cal;
- const pwm_flash_t *bound_flash;
const pwm_input_getters_t *bound_getters;
} pwm_runtime_t;
-/* ── Calibration (TABLE[RWA4]+offset) ───────────────────────────────── */
+/* ── Calibration (decoded ROM values) ───────────────────────────────── */
struct pwm_calibration {
- int16_t target_minimum; /* CAL+0x11E */
- /* Y-table pointers for each submap pair. Each field originally held a
- * 16-bit ROM address (CAL+0x184/186/188). [0]=RPM×InjQty, [1]=RPM×Temp,
- * [2]=RPM×AngleDec — see src/submap_inputvars.txt */
- const int16_t *y_pair[3]; /* CAL+0x184/186/188 */
- /* PI shaping (CAL+0x00FE…0x0116) */
- int16_t pi_upper_breakpoint, pi_lower_breakpoint;
- int16_t pi_center_slope, pi_upper_outer_slope, pi_lower_outer_slope;
- int16_t pi_upper_integrator_gain, pi_center_integrator_gain, pi_lower_integrator_gain;
+ /* Scalars (from families/t06211/cal_offsets.py FLASH_OFFSETS) */
+ int16_t large_pos_error_thresh; /* cal+0x10E */
+ int16_t large_neg_error_thresh; /* cal+0x110 */
+ int16_t pi_low_clamp; /* cal+0x120 = -512 — Block-4 low clamp + open-loop lower bound */
+ int16_t pi_high_clamp; /* cal+0x124 = +1707 — Block-4 high clamp */
- /* compute_closed_loop_correction tuning */
- int16_t closed_loop_gain_const; /* CAL+0x0156 */
+ /* CAN-decoded setpoint (FUN_64c3) cal constants */
+ int16_t b_fb_kw_upper_bound; /* cal+0x004 = +7680 — raw b_fb_kw upper sanity bound */
+ int16_t b_fb_kw_lower_bound; /* cal+0x006 = -768 — raw b_fb_kw lower sanity bound */
- /* fast_recovery tuning */
- int16_t error_persist_init_count; /* CAL+0x0108 */
- int16_t recovery_rpm_threshold; /* CAL+0x011A */
+ /* setpoint_offset — static bias added after halving raw b_fb_kw.
+ * Family-1 analog: CAL+0x0052 - CAL+0x0054 (compact_src/pwm.h:210).
+ * For t06211, FUN_6b7b computes the equivalent as cal+0x4c - cal+0x4e
+ * (= 3499 - 4156 = -657) at boot and stores at RAM[0x150].
+ * Copied into runtime.setpoint_offset_150 by runtime_reset. */
+ int16_t setpoint_offset; /* = cal+0x4c - cal+0x4e */
+ int16_t target_5e_min_clamp; /* cal+0x122 = -512 — RW5E lower clamp */
+ int16_t can_aux_12e_max; /* cal+0x002 — upper bound for RAM[0x12e] (FUN_649e) */
+ int16_t error_thresh_114; /* cal+0x114 */
+ int16_t pi_thresh_116; /* cal+0x116 */
+ /* pi_state_118 saturation threshold — when the recovery counter
+ * reaches this, FUN_66a8 latches bit0 of system_flags_110, zeroes
+ * the counter, and reloads pi_state_c2 from error_thresh_114.
+ * Boot-cached at RAM[0x02c0] by FUN_6b7b:0x6bd4. ROM literal 0x0320 = 800. */
+ int16_t pi_sat_count_threshold; /* cal+0x112 */
+ int16_t rpm_threshold_11E; /* cal+0x11E — RPM gate inside FUN_66a8 */
+ /* Closed-loop entry RPM floor (FUN_67c4:0x67d9). Sourced from
+ * RAM[0x605c] (flash mirror; ROM literal 0x01A4 = 420). */
+ int16_t pi_cl_rpm_floor;
- /* Setpoint offset latched once at init by FUN_8643 (0x867d):
- * RAM[0x0150] = CAL[0x52] - (CAL[0x54] + RAM[0x0430])
- * RAM[0x0430] is observed as always 0 at runtime, so the effective
- * constant is CAL[0x52] - CAL[0x54]. */
- int16_t setpoint_offset; /* CAL+0x0052 - CAL+0x0054 (RAM 0x0150) */
+ int16_t pwm_detail_x0; /* cal+0x0EE */
+ int16_t pwm_detail_x1; /* cal+0x0F0 */
+ int16_t pwm_cached_ptr_0F2; /* cal+0x0F2 — first RPM-window breakpoint (legacy scalar) */
+ int16_t pwm_cached_ptr_102; /* cal+0x102 — window halfwidth (legacy scalar) */
+ int16_t pwm_const_104; /* cal+0x104 */
+
+ /* RPM-window matching for pwm_period slew (ROM 0x538b-0x5575).
+ * Eight breakpoints at cal+0xF2 define four bands (lo,hi); the
+ * cal+0x102 halfwidth adds hysteresis. RPM INSIDE any band drives
+ * pwm_period toward pwm_period_min (→ non-zero shape contribution
+ * adds ~49 duty ticks). RPM OUTSIDE all bands drives pwm_period
+ * toward pwm_period_max (→ zero contribution; Y-table floor). */
+ int16_t pwm_rpm_windows[8]; /* cal+0xF2 — 4 (lo,hi) pairs */
+ int16_t pwm_window_halfwidth; /* cal+0x102 — hysteresis halfwidth */
+ /* Per-cycle slew step magnitude for pwm_period. Sourced from
+ * cal+0x104 (= 354); cached to RAM[0x02ec] at FUN_5314:0x53b9 and
+ * read at the Phase 1 and Phase 3 sites. Aliases pwm_const_104 —
+ * same offset, semantic name. */
+ int16_t pwm_slew_step; /* cal+0x104 — slew magnitude */
+
+ const int16_t *pwm_y_table; /* cal+0x154 (indirect) */
+ const int16_t *shape_y_table; /* cal+0x15E (indirect) */
+
+ int16_t closed_loop_gain_const; /* cached at ROM 0x6056 = 0x000A */
+
+ uint16_t pwm_period_min; /* hypothesised; see cal_tables_rom.c */
+ uint16_t pwm_period_max; /* hypothesised */
+
+ /* PWM duty clamp bounds — RAM[0x6058]/RAM[0x605a] flash mirrors.
+ * Defaults 0x00CD/0x0F32 match the default family's pwm_min/pwm_max. */
+ uint16_t pwm_min; /* RAM[0x6058] cache */
+ uint16_t pwm_max; /* RAM[0x605a] cache */
};
-/* ── Flash-resident tables ──────────────────────────────────────────── */
-struct pwm_flash {
- int16_t max_error_x[INTERP_MAX_ENTRIES];
- int16_t max_error_y[INTERP_MAX_ENTRIES];
- uint16_t max_error_count;
- int16_t max_error_input_addr;
- int16_t request_upper_limit; /* FLASH:9734 */
- int16_t large_pos_error_thresh; /* FLASH:971a */
- int16_t large_neg_error_thresh; /* FLASH:971c */
- int16_t inj_qty_demand_thresh; /* FLASH:9722 */
- uint16_t rpm_breakpoints[8]; /* FLASH:96fe */
- uint16_t rpm_values[8]; /* FLASH:970e */
- int16_t shape_scale_src; /* FLASH:9710 */
- uint16_t period_max_limit, period_min_limit; /* FLASH:96fa / 96fc */
- const int16_t *pwm_y_table; /* FLASH:9768 — 2D Y-table */
- const int16_t *shape_y_table; /* FLASH:9772 — 1D Y-table */
-};
-
-/* ── Submap descriptor (mirrors calibration-block layout) ───────────── */
+/* ── Submap descriptor ──────────────────────────────────────────────── */
struct pwm_submap_descr {
uint16_t flags; /* +0 */
const int16_t *input_ptr; /* +2 (bound at runtime) */
uint16_t count; /* +4 */
const int16_t *x; /* +6 */
- uint16_t input_addr; /* original address (for input_ptr binding) */
+ uint16_t input_addr;
};
-/** Bind submap descriptor input_ptr fields to the matching source in rt.
- * Most descriptors point at fields in rt->inputs (external signals), but
- * input_addr 0x0046 resolves to rt->active_request — in the ROM that RAM
- * address is RW46, which finalize_angle_request writes with the PI
- * output at its exit (disasm 0x73f4), so the pwm_B submap sees the
- * latest active_request when pwm_output_compute runs.
- * Recognised input_addr values: 0x0040, 0x0042, 0x0044, 0x0046, 0x0142,
- * 0x0146. Others are bound to NULL. */
-static inline void pwm_bind_submap_inputs(pwm_runtime_t *rt,
- pwm_submap_descr_t *descrs,
- uint16_t n)
+/** Bind descriptor input_ptr fields to rt inputs. */
+static inline void pwm_bind_submap_inputs(
+ pwm_runtime_t *rt,
+ pwm_submap_descr_t *descrs,
+ uint16_t n)
{
for (uint16_t i = 0; i < n; i++) {
switch (descrs[i].input_addr) {
case 0x0040: descrs[i].input_ptr = (const int16_t *)&rt->inputs.rpm; break;
- case 0x0042: descrs[i].input_ptr = &rt->inputs.angle_dec_cmd; break;
case 0x0044: descrs[i].input_ptr = &rt->inputs.inj_qty_demand; break;
case 0x0046: descrs[i].input_ptr = &rt->active_request; break;
case 0x0142: descrs[i].input_ptr = (const int16_t *)&rt->inputs.supply_voltage; break;
- case 0x0146: descrs[i].input_ptr = &rt->inputs.temperature; break;
default: descrs[i].input_ptr = NULL; break;
}
}
@@ -266,34 +319,69 @@ static inline void pwm_bind_submap_inputs(pwm_runtime_t *rt,
* PUBLIC API
* ══════════════════════════════════════════════════════════════════════ */
-/** One-time setup. Zero-inits internal state, applies PI shaping from
- * cal, and caches cal/flash/getters into rt for later use by pwm_service.
- * None of the pointer arguments may be NULL. */
+/** pwm_flash_t — placeholder for family-1 API parity. Family-1 has Y-tables
+ * in a separate `pwm_flash_t` struct; t06211 keeps them inside
+ * pwm_calibration_t so this struct is empty. Kept so FBKW.c / callers
+ * can pass &pwm_flash_rom without the call failing. */
+typedef struct pwm_flash { char _unused; } pwm_flash_t;
+
+/** 4-argument init matching family-1's pwm_init signature. The `flash`
+ * pointer is accepted (must be non-NULL — pass &pwm_flash_rom) and
+ * ignored by the t06211 pipeline. */
void pwm_init(pwm_runtime_t *rt,
const pwm_calibration_t *cal,
const pwm_flash_t *flash,
const pwm_input_getters_t *getters);
-/** Per-cycle update. Calls each getter to populate rt->inputs, then
- * runs the 6-stage control pipeline:
- * 1. max_cl_error lookup
- * 2. setpoint computation
- * 3. supervisor (error + CL-enable + clamping)
- * 4. closed-loop correction + angle estimate
- * 5. PI controller
- * 6. PWM duty + HW register split
- * Outputs are written to rt (pwm_on_time, pwm_off_time, pwm_period, etc).
- */
void pwm_service(pwm_runtime_t *rt);
-/** Utility: 1D descending-X piecewise-linear lookup (helper_from_cal). */
-int16_t pwm_interp_lookup(const int16_t *x, const int16_t *y,
- uint16_t n, int16_t in);
+/** @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; }
-/* ── Default & ROM-extracted data ───────────────────────────────────── */
-extern const pwm_calibration_t pwm_cal_default;
-extern const pwm_flash_t pwm_flash_default;
+/** Utility: 1D descending-X piecewise-linear lookup. */
+int16_t pwm_interp_lookup(const int16_t *x, const int16_t *y,
+ uint16_t n, int16_t in);
+
+/** Utility: bypass-PI bilinear LUT — eval(pwm_A, rpm), eval(pwm_B, fbkw),
+ * combine over Y-table, then apply [205, 3890] clamp. Use this to query
+ * the ROM Y-table directly as a (rpm, fbkw) lookup, skipping the PI
+ * controller entirely. Returns the clamped duty. */
+uint16_t pwm_lut_duty(const pwm_calibration_t *cal,
+ uint16_t rpm, int16_t fbkw);
+
+/* ── ROM-decoded cal + flash placeholder (defined in cal_tables_rom.c) ── */
extern const pwm_calibration_t pwm_cal_rom;
extern const pwm_flash_t pwm_flash_rom;
+/** Descriptor array indices. */
+enum pwm_submap_id {
+ PWM_SUBMAP_SETPOINT_INTERP = 0,
+ PWM_SUBMAP_PWM_A = 1,
+ PWM_SUBMAP_PWM_B = 2,
+ PWM_SUBMAP_SHAPE_EVAL = 3,
+ PWM_SUBMAP_COUNT = 4
+};
+
+extern pwm_submap_descr_t pwm_submap_descrs[PWM_SUBMAP_COUNT];
+extern const int16_t *pwm_submap_y_of(uint16_t idx);
+
#endif /* PWM_H */
diff --git a/Core/Advance_Control/pwm_004002.c b/Core/Advance_Control/pwm_004002.c
new file mode 100644
index 0000000..7d36c8c
--- /dev/null
+++ b/Core/Advance_Control/pwm_004002.c
@@ -0,0 +1,543 @@
+/**
+ * @file pwm.c
+ * @brief Compact single-file implementation of the VP44 PWM solenoid control
+ * pipeline. Combines interpolation, setpoint, supervisor, PI, PWM
+ * output, orchestrator, ported ex-external routines, and default
+ * cal data.
+ *
+ * Every arithmetic choice (MUL vs MULU, SHRA vs SHR, JGE vs JC) mirrors the
+ * MCS-96 assembly — see the original src/*.c files for per-block address
+ * annotations and disassembly/*.txt for the source.
+ */
+#include "pwm.h"
+#include "cal_tables_rom.h"
+
+/* Forward decls for ported ex-external routines (defined below). */
+static int16_t s_eval (const pwm_submap_descr_t *descr,
+ pwm_interp_slot_t *slot);
+static int16_t s_combine (const int16_t *y_base,
+ const pwm_interp_slot_t *sx,
+ const pwm_interp_slot_t *sy);
+static int16_t s_refine (const int16_t *y_base,
+ const pwm_interp_slot_t *slot);
+static void s_correction(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal);
+static void s_recovery (pwm_runtime_t *rt,
+ const pwm_calibration_t *cal,
+ uint16_t rpm);
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Runtime init (file-static — exposed via pwm_init)
+ * ═════════════════════════════════════════════════════════════════════ */
+static void runtime_reset(pwm_runtime_t *rt)
+{
+ memset(rt, 0, sizeof *rt);
+ rt->mode_flags = 0x03; /* first_call + outer */
+ rt->min_rpm_openloop = 0x01A4; /* 420 RPM */
+ rt->upper_breakpoint = 107;
+ rt->lower_breakpoint = (int16_t)0xFF95;
+ rt->center_slope = 2560;
+ rt->upper_outer_slope = 5120;
+ rt->lower_outer_slope = 5120;
+ rt->upper_integrator_gain = 1024;
+ rt->center_integrator_gain = 1024;
+ rt->lower_integrator_gain = 2048;
+ rt->pwm_period = 29851;
+ rt->shape_scale = 24;
+ rt->pwm_min = 205;
+ rt->pwm_max = 3890;
+ /* Observed defaults from Ghidra annotations on compute_closed_loop_correction
+ * (0x01F4, 0x028A) and fast_recovery (0x01F4). TODO: locate flash source. */
+ rt->pos_error_normalizer = 500; /* 0x01F4 */
+ rt->neg_error_normalizer = 650; /* 0x028A */
+ rt->recovery_count_threshold = 500; /* 0x01F4 */
+}
+
+static void apply_calibration(pwm_runtime_t *rt, const pwm_calibration_t *c)
+{
+ rt->upper_breakpoint = c->pi_upper_breakpoint;
+ rt->lower_breakpoint = c->pi_lower_breakpoint;
+ rt->center_slope = c->pi_center_slope;
+ rt->upper_outer_slope = c->pi_upper_outer_slope;
+ rt->lower_outer_slope = c->pi_lower_outer_slope;
+ rt->upper_integrator_gain = c->pi_upper_integrator_gain;
+ rt->center_integrator_gain = c->pi_center_integrator_gain;
+ rt->lower_integrator_gain = c->pi_lower_integrator_gain;
+ rt->setpoint_offset = c->setpoint_offset;
+}
+
+void pwm_init(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal,
+ const pwm_flash_t *flash,
+ const pwm_input_getters_t *getters)
+{
+ runtime_reset(rt);
+ apply_calibration(rt, cal);
+ rt->bound_cal = cal;
+ rt->bound_flash = flash;
+ rt->bound_getters = getters;
+ /* Resolve each descriptor's input_ptr into this rt's inputs block, so
+ * s_eval() can dereference live values. Descriptor order/IDs are fixed
+ * by the PWM_SUBMAP_* enum in cal_tables_rom.h. */
+ pwm_bind_submap_inputs(rt, pwm_submap_descrs, PWM_SUBMAP_COUNT);
+}
+
+static void read_inputs(pwm_runtime_t *rt)
+{
+ const pwm_input_getters_t *g = rt->bound_getters;
+ void *ctx = g->ctx;
+ rt->inputs.ckp_in = g->ckp_in (ctx);
+ rt->inputs.rpm = g->rpm (ctx);
+ rt->inputs.angle_dec_cmd = g->angle_dec_cmd (ctx);
+ rt->inputs.inj_qty_demand = g->inj_qty_demand(ctx);
+ rt->inputs.b_fb_kw = g->b_fb_kw (ctx);
+ rt->inputs.state_130 = g->state_130 (ctx);
+ rt->inputs.supply_voltage = g->supply_voltage(ctx);
+ rt->inputs.temperature = g->temperature (ctx);
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Interpolation — helper_from_cal (0x838b–0x83fa)
+ * Descending-X piecewise linear, signed MUL + signed DIV.
+ * ═════════════════════════════════════════════════════════════════════ */
+int16_t pwm_interp_lookup(const int16_t *x, const int16_t *y,
+ uint16_t n, int16_t in)
+{
+ if (in >= x[0]) return y[0];
+ if (in <= x[n - 1u]) return y[n - 1u];
+
+ /* One-step binary midpoint jump, then linear scan. */
+ uint16_t mid = (n & 0xFFFEu) / 2u;
+ uint16_t k = (in < x[mid]) ? (uint16_t)(mid + 1u) : 1u;
+ while (k < n && in < x[k]) k++;
+
+ int16_t num = (int16_t)(in - x[k]);
+ int16_t dx = (int16_t)(x[k] - x[k - 1u]);
+ int16_t dy = (int16_t)(y[k] - y[k - 1u]);
+ int32_t prod = MUL_S16(num, dy);
+ return (int16_t)((int16_t)(prod / (int32_t)dx) + y[k]);
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Setpoint — precompute_control_support_maps (0x8e36–0x8f1f)
+ * ═════════════════════════════════════════════════════════════════════ */
+static void setpoint_compute(pwm_runtime_t *rt, const pwm_calibration_t *cal)
+{
+ /* Pair 0: slot_x=sp0_A(rpm), slot_y=sp0_B(inj_qty_demand) */
+ s_eval(&pwm_submap_descrs[PWM_SUBMAP_SP0_A], &rt->setpoint_slot_x);
+ s_eval(&pwm_submap_descrs[PWM_SUBMAP_SP0_B], &rt->setpoint_slot_y);
+ rt->compensation_angle =
+ s_combine(cal->y_pair[0], &rt->setpoint_slot_x, &rt->setpoint_slot_y);
+
+ /* Pair 1: slot_x reused (still sp0_A rpm), slot_y=sp1_B(temperature) */
+ s_eval(&pwm_submap_descrs[PWM_SUBMAP_SP1_B], &rt->setpoint_slot_y);
+ rt->compensation_angle = (int16_t)(rt->compensation_angle +
+ s_combine(cal->y_pair[1], &rt->setpoint_slot_x, &rt->setpoint_slot_y));
+
+ /* Pair 2: slot_x=sp2_A(rpm), slot_y=sp2_B(angle_dec_cmd) */
+ s_eval(&pwm_submap_descrs[PWM_SUBMAP_SP2_A], &rt->setpoint_slot_x);
+ s_eval(&pwm_submap_descrs[PWM_SUBMAP_SP2_B], &rt->setpoint_slot_y);
+ rt->compensation_angle = (int16_t)(rt->compensation_angle +
+ s_combine(cal->y_pair[2], &rt->setpoint_slot_x, &rt->setpoint_slot_y));
+
+ int16_t sum = (int16_t)(rt->inputs.b_fb_kw + rt->compensation_angle);
+ int16_t half = shra16(sum, 1); /* SHRA — signed halve */
+ rt->pre_offset_target = half;
+
+ int16_t t = (int16_t)(half + rt->setpoint_offset);
+ if (t < cal->target_minimum) t = cal->target_minimum;
+ rt->target = t;
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Supervisor — update_closed_loop_supervisor (0x929b–0x92f1)
+ * One-sided (upper-only) clamp on cl_error_delta.
+ * ═════════════════════════════════════════════════════════════════════ */
+static void supervisor_update(pwm_runtime_t *rt)
+{
+ if (rt->reset_flag == 1) {
+ rt->reset_flag = 0;
+ rt->cl_enable_counter = 0;
+ rt->supervisor_state_17e = 0;
+ /* ROM snapshots integrator_hi → 0x033e here; confirmed dead-store, dropped */
+ } else {
+ rt->cl_enable_counter = (int16_t)(rt->cl_enable_counter + 1);
+ }
+ int16_t err = (int16_t)(rt->target - rt->inputs.ckp_in);
+ err = (int16_t)(err + rt->angle_error_shaped);
+ rt->cl_error_delta = err;
+ if (err > rt->max_cl_error) rt->cl_error_delta = rt->max_cl_error;
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * PI controller — finalize_angle_request (0x713b–0x7414)
+ * ═════════════════════════════════════════════════════════════════════ */
+static void shape_error(pwm_runtime_t *rt)
+{
+ int16_t e = rt->angle_error;
+ int16_t bp, slope, gain;
+
+ if (e > rt->upper_breakpoint) {
+ rt->mode_flags |= MODE_OUTER_REGION;
+ bp = rt->upper_breakpoint; slope = rt->upper_outer_slope;
+ gain = rt->upper_integrator_gain;
+ } else if (e < rt->lower_breakpoint) {
+ rt->mode_flags |= MODE_OUTER_REGION;
+ bp = rt->lower_breakpoint; slope = rt->lower_outer_slope;
+ gain = rt->lower_integrator_gain;
+ } else {
+ rt->mode_flags &= (uint8_t)~MODE_OUTER_REGION;
+ rt->integrator_gain = rt->center_integrator_gain;
+ rt->angle_error_shaped =
+ (int16_t)shra32(MUL_S16(rt->center_slope, e), 8);
+ return;
+ }
+ rt->integrator_gain = gain;
+ int32_t outer = MUL_S16((int16_t)(e - bp), slope);
+ int32_t center = MUL_S16(rt->center_slope, bp);
+ rt->angle_error_shaped = (int16_t)shra32(outer + center, 8);
+}
+
+static void pi_controller_update(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal,
+ const pwm_flash_t *flash)
+{
+ const uint16_t rpm = rt->inputs.rpm;
+ const int16_t inj_qty_demand = rt->inputs.inj_qty_demand;
+
+ /* A. Entry conditions — all three must hold for closed-loop. */
+ bool open_loop = (rt->system_flags_110 & 0x20) != 0
+ || rt->inputs.state_130 < 0
+ || (uint16_t)rpm < (uint16_t)rt->min_rpm_openloop;
+
+ if (open_loop) {
+ rt->mode_flags |= MODE_FIRST_CALL;
+ rt->active_request = rt->target;
+ if (rt->active_request < flash->request_upper_limit)
+ rt->active_request = flash->request_upper_limit;
+ goto rotate_flags;
+ }
+
+ /* C. Error */
+ rt->angle_error = (int16_t)(rt->target - rt->estimated_angle);
+
+ /* D/E/F. Large-positive / large-negative / normal-range paths */
+ if (rt->angle_error > flash->large_pos_error_thresh) {
+ rt->mode_flags = (uint8_t)((rt->mode_flags | MODE_LARGE_POS) & ~MODE_LARGE_NEG);
+ s_recovery(rt, cal, rpm);
+ } else if (rt->angle_error < flash->large_neg_error_thresh) {
+ rt->mode_flags = (uint8_t)((rt->mode_flags | MODE_LARGE_NEG) & ~MODE_LARGE_POS);
+ if ((uint16_t)inj_qty_demand > (uint16_t)flash->inj_qty_demand_thresh)
+ s_recovery(rt, cal, rpm);
+ else
+ rt->recovery_counter = 0;
+ } else {
+ rt->mode_flags &= (uint8_t)~(MODE_LARGE_POS | MODE_LARGE_NEG);
+ if ((uint16_t)rt->error_persist_counter > 0u)
+ rt->error_persist_counter = (int16_t)(rt->error_persist_counter - 1);
+ if (rt->error_persist_counter == 0)
+ rt->system_flags_110 &= (uint8_t)~0x01;
+ if (rt->recovery_counter > 0)
+ rt->recovery_counter = (int16_t)(rt->recovery_counter - 1);
+ }
+
+ /* G. Shape error */
+ shape_error(rt);
+
+ /* H. Integrator — first-call init or anti-windup-gated accumulate */
+ if (rt->mode_flags & MODE_FIRST_CALL) {
+ int16_t lo_prod = (int16_t)MUL_S16(rt->first_call_gain, rt->angle_error);
+ int16_t init_hi = (int16_t)(shra16(lo_prod, 4) + rt->inputs.ckp_in);
+ rt->integrator_state = (int32_t)(((uint32_t)(uint16_t)init_hi << 16) |
+ INTEG_LO(rt->integrator_state));
+ rt->mode_flags &= (uint8_t)~MODE_FIRST_CALL;
+ } else {
+ bool freeze = (rt->angle_error < 0)
+ ? (rt->mode_flags & MODE_NEG_SAT) != 0
+ : (rt->mode_flags & MODE_POS_SAT) != 0;
+ if (!freeze) {
+ int32_t inc = MUL_S16(rt->angle_error, rt->integrator_gain) << 4;
+ rt->integrator_state += inc;
+ }
+ }
+
+ /* I. Output + saturation clamps (signed) */
+ rt->active_request = (int16_t)(rt->angle_error_shaped +
+ INTEG_HI(rt->integrator_state));
+ if (rt->active_request < flash->request_upper_limit) {
+ rt->active_request = flash->request_upper_limit;
+ rt->mode_flags = (uint8_t)((rt->mode_flags | MODE_NEG_SAT) & ~MODE_POS_SAT);
+ } else if (rt->active_request > rt->max_cl_error) {
+ rt->active_request = rt->max_cl_error;
+ rt->mode_flags = (uint8_t)((rt->mode_flags | MODE_POS_SAT) & ~MODE_NEG_SAT);
+ } else {
+ rt->mode_flags &= (uint8_t)~(MODE_NEG_SAT | MODE_POS_SAT);
+ }
+
+rotate_flags:
+ /* J. Rotate bits 4-5 into bits 6-7 (prev_sat_shadow). Consumed next
+ * cycle by fast_recovery (FUN_70ae at 0x70ae/0x70b6) via the pattern
+ * sustained = (mode_flags << 2) & mode_flags
+ * to detect two-cycle sustained saturation. [0x73f4–0x7414] */
+ rt->mode_flags = (uint8_t)((rt->mode_flags & 0x3Fu) |
+ (((uint16_t)rt->mode_flags << 2) & 0xC0u));
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * PWM output — compute_pwm_duty_from_maps (0x7415–0x7760)
+ * Nearly all UNSIGNED arithmetic (MULU, SHR, DIVU, JC/JNH).
+ * ═════════════════════════════════════════════════════════════════════ */
+static bool rpm_in_any_window(uint16_t rpm, const uint16_t *bp,
+ uint16_t lo_pad, uint16_t hi_pad)
+{
+ for (int i = 0; i < 4; i++) {
+ uint16_t lo = (uint16_t)(bp[2 * i] - lo_pad);
+ uint16_t hi = (uint16_t)(bp[2 * i + 1] + hi_pad);
+ if (rpm > lo && rpm < hi) return true;
+ }
+ return false;
+}
+
+static void pwm_output_compute(pwm_runtime_t *rt, const pwm_flash_t *flash)
+{
+ const uint16_t rpm = rt->inputs.rpm;
+
+ /* A. Base duty from submap pair — slot_x=pwm_A(rpm),
+ * slot_y=pwm_B(active_request) [0x0046 = PI output feed-forward] */
+ s_eval(&pwm_submap_descrs[PWM_SUBMAP_PWM_A], &rt->pwm_slot_x);
+ s_eval(&pwm_submap_descrs[PWM_SUBMAP_PWM_B], &rt->pwm_slot_y);
+ rt->pwm_duty = (uint16_t)s_combine(
+ flash->pwm_y_table, &rt->pwm_slot_x, &rt->pwm_slot_y);
+
+ /* B. Duty status flag (unsigned) */
+ rt->pwm_status_flag = (rt->pwm_duty < 0x29u) ? 1u
+ : (rt->pwm_duty > 0xFD7u) ? 2u : 0u;
+
+ /* C/D. Coarse RPM window — shape_intermediate = +shape_scale if in any */
+ rt->shape_scale = flash->shape_scale_src;
+ bool coarse = rpm_in_any_window(rpm, flash->rpm_breakpoints, 0, 0);
+
+ if (coarse) {
+ rt->shape_intermediate = rt->shape_scale;
+ } else {
+ /* E. Detail pass — expanded windows padded by rpm_values[0] */
+ uint16_t pad = flash->rpm_values[0];
+ bool detail = rpm_in_any_window(rpm, flash->rpm_breakpoints, pad, pad);
+ if (!detail && rt->pwm_period < flash->period_max_limit)
+ rt->shape_intermediate = (int16_t)(-rt->shape_scale);
+ /* else: shape_intermediate unchanged */
+ }
+
+ /* F. Period adjust + unsigned clamp to [min, max] */
+ rt->pwm_period = (uint16_t)(rt->pwm_period - (uint16_t)rt->shape_intermediate);
+ if (rt->pwm_period > flash->period_max_limit) rt->pwm_period = flash->period_max_limit;
+ if (rt->pwm_period < flash->period_min_limit) rt->pwm_period = flash->period_min_limit;
+
+ /* G. Shape refinement (signed clamp to [0, 0x199]) — shape_eval(supply_voltage) */
+ s_eval(&pwm_submap_descrs[PWM_SUBMAP_SHAPE_EVAL], &rt->pwm_slot_x);
+ rt->shape_output = s_refine(flash->shape_y_table, &rt->pwm_slot_x);
+ if (rt->shape_output < 0) rt->shape_output = 0;
+ if (rt->shape_output > (int16_t)0x199) rt->shape_output = (int16_t)0x199;
+
+ /* H. Duty refinement — all UNSIGNED (SHR, MULU, DIVU) */
+ uint16_t range = (uint16_t)((uint16_t)(flash->period_max_limit
+ - flash->period_min_limit) >> 8);
+ rt->shape_scale = (int16_t)range;
+ uint16_t delta = (uint16_t)((uint16_t)(flash->period_max_limit
+ - rt->pwm_period) >> 8);
+ uint32_t prod = MULU_U16(delta, (uint16_t)rt->shape_output);
+ uint16_t trunc = (uint16_t)(prod & 0xFFFFu); /* assembly CLR RW1E */
+ uint16_t quot = range ? (uint16_t)(trunc / range) : 0u;
+ rt->pwm_duty = (uint16_t)(rt->pwm_duty + quot);
+
+ /* I. Absolute duty clamp (unsigned) */
+ if (rt->pwm_duty < rt->pwm_min) rt->pwm_duty = rt->pwm_min;
+ if (rt->pwm_duty > rt->pwm_max) rt->pwm_duty = rt->pwm_max;
+
+ /* J. HW register split — critical section in assembly (DI/EI) */
+ uint32_t hw = MULU_U16(rt->pwm_period, rt->pwm_duty);
+ rt->pwm_on_time = (uint16_t)(hw / 0xFFFu);
+ rt->pwm_off_time = (uint16_t)(rt->pwm_period - rt->pwm_on_time);
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Orchestrator — main_pwm_control_service (0x8cc9–0x8cf1)
+ * + inlined publish_closed_loop_request (0x937d–0x938f)
+ * ═════════════════════════════════════════════════════════════════════ */
+void pwm_service(pwm_runtime_t *rt)
+{
+ /* 0. Refresh external inputs via user getters. */
+ read_inputs(rt);
+
+ const pwm_calibration_t *cal = rt->bound_cal;
+ const pwm_flash_t *flash = rt->bound_flash;
+
+ /* 1. Max-error interpolation */
+ if (flash->max_error_count > 0)
+ rt->max_cl_error = pwm_interp_lookup(flash->max_error_x,
+ flash->max_error_y,
+ flash->max_error_count,
+ (int16_t)rt->inputs.rpm);
+ /* 2. Target setpoint */
+ setpoint_compute(rt, cal);
+
+ /* 3. Supervisor */
+ supervisor_update(rt);
+
+ /* 4. Publish correction + angle estimate */
+ s_correction(rt, cal);
+ rt->estimated_angle = (int16_t)(rt->inputs.ckp_in + rt->angle_offset);
+
+ /* 5. PI controller */
+ pi_controller_update(rt, cal, flash);
+
+ /* 6. PWM duty + HW registers */
+ pwm_output_compute(rt, flash);
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * External function impls (ported 1:1 from disassembly)
+ * ═════════════════════════════════════════════════════════════════════ */
+/* real_eval_submap (disassembled 81db–8236). */
+static int16_t s_eval(const pwm_submap_descr_t *descr, pwm_interp_slot_t *slot)
+{
+ int16_t input_val = (descr && descr->input_ptr) ? *descr->input_ptr : 0;
+ uint16_t count = descr ? descr->count : 0u;
+ const int16_t *x = descr ? descr->x : NULL;
+ if (count == 0u || x == NULL) {
+ memset(slot, 0, sizeof *slot);
+ return 0;
+ }
+ slot->row_stride = (int16_t)(count * 2u);
+ uint16_t k;
+ for (k = 1; k < count; k++) {
+ if (input_val >= x[k]) break;
+ }
+ if (k == 1 && input_val >= x[0]) {
+ slot->x_interval = 2; slot->x_offset = 2; slot->y_byte_off = 2;
+ return 0;
+ }
+ if (k >= count) k = (uint16_t)(count - 1u);
+ slot->x_interval = (int16_t)(x[k - 1] - x[k]);
+ slot->x_offset = (int16_t)(input_val - x[k]);
+ slot->y_byte_off = (int16_t)(k * 2u);
+ return 0;
+}
+
+/* real_combine_submaps (disassembled 8258–82b4). Bilinear over row-major
+ * [B_count][A_count] int16 Y-table. slot_x->row_stride is A-axis byte stride. */
+static int16_t s_combine(const int16_t *y_base,
+ const pwm_interp_slot_t *sx,
+ const pwm_interp_slot_t *sy)
+{
+ if (y_base == NULL || sx->x_interval == 0 || sy->x_interval == 0) return 0;
+ int32_t row_off = MUL_S16(sy->y_byte_off, sx->row_stride) / 2;
+ const int16_t *yp = (const int16_t *)((const uint8_t *)y_base
+ + row_off + sx->y_byte_off);
+ int16_t y_here = *yp;
+ int16_t y_prev = *(const int16_t *)((const uint8_t *)yp - 2);
+ int32_t diff_a = MUL_S16((int16_t)(y_prev - y_here), sx->x_offset);
+ int16_t rowB = (int16_t)(y_here + (int32_t)(diff_a / (int32_t)sx->x_interval));
+
+ const int16_t *yp_p = (const int16_t *)((const uint8_t *)yp - sx->row_stride);
+ int16_t y_here_p = *yp_p;
+ int16_t y_prev_p = *(const int16_t *)((const uint8_t *)yp_p - 2);
+ int32_t diff_a_p = MUL_S16((int16_t)(y_prev_p - y_here_p), sx->x_offset);
+ int16_t rowBp = (int16_t)(y_here_p
+ + (int32_t)(diff_a_p / (int32_t)sx->x_interval));
+
+ int32_t diff_b = MUL_S16((int16_t)(rowBp - rowB), sy->x_offset);
+ return (int16_t)(rowB + (int32_t)(diff_b / (int32_t)sy->x_interval));
+}
+
+/* compute_closed_loop_correction (disassembled 92f2–9339). */
+static void s_correction(pwm_runtime_t *rt, const pwm_calibration_t *cal)
+{
+ int16_t correction = 0;
+ if ((uint8_t)(rt->cl_enable_counter & 0xFF) != 0) {
+ int16_t normalizer = (rt->cl_error_delta > 0)
+ ? rt->pos_error_normalizer
+ : rt->neg_error_normalizer;
+ int32_t product = MUL_S16(rt->cl_error_delta,
+ cal->closed_loop_gain_const);
+ correction = (int16_t)((product << 4) / (int32_t)normalizer);
+ }
+ rt->cl_correction_raw = correction;
+ rt->supervisor_state_17e = (int16_t)(rt->supervisor_state_17e + correction);
+ rt->angle_offset = shra16(rt->supervisor_state_17e, 4);
+}
+
+/* real_refine_submap (disassembled 8237–8257). 1D variant of combine. */
+static int16_t s_refine(const int16_t *y_base, const pwm_interp_slot_t *slot)
+{
+ if (y_base == NULL || slot->x_interval == 0) return 0;
+ const int16_t *yp = (const int16_t *)((const uint8_t *)y_base + slot->y_byte_off);
+ int16_t y_here = *yp;
+ int16_t y_prev = *(const int16_t *)((const uint8_t *)yp - 2);
+ int32_t diff = MUL_S16((int16_t)(y_prev - y_here), slot->x_offset);
+ return (int16_t)(y_here + (int32_t)(diff / (int32_t)slot->x_interval));
+}
+
+/* fast_recovery FUN_70ae (disassembled 70ae–713a). */
+static void s_recovery(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal,
+ uint16_t rpm)
+{
+ uint16_t mf = (uint16_t)rt->mode_flags;
+ uint16_t sustained = (uint16_t)(((mf << 2) & mf));
+ if (sustained > 0x30u) {
+ if ((uint16_t)rt->recovery_counter
+ >= (uint16_t)rt->recovery_count_threshold) {
+ rt->system_flags_110 |= 0x01u;
+ rt->recovery_counter = 0;
+ rt->error_persist_counter = cal->error_persist_init_count;
+ } else if ((int16_t)rpm > cal->recovery_rpm_threshold
+ && (rt->system_flags_110 & 0x10u) == 0u
+ && (rt->system_flags_110 & 0x20u) == 0u
+ && rt->error_persist_counter == 0) {
+ rt->recovery_counter = (int16_t)(rt->recovery_counter + 1);
+ }
+ } else if ((rt->system_flags_110 & 0x01u) == 0u) {
+ rt->recovery_counter = 0;
+ }
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Default calibration (TODO-placeholder values)
+ * ═════════════════════════════════════════════════════════════════════ */
+const pwm_calibration_t pwm_cal_default = {
+ .target_minimum = 0,
+ .y_pair = { NULL, NULL, NULL },
+ .pi_upper_breakpoint = 107,
+ .pi_lower_breakpoint = (int16_t)0xFF95,
+ .pi_center_slope = 2560,
+ .pi_upper_outer_slope = 5120,
+ .pi_lower_outer_slope = 5120,
+ .pi_upper_integrator_gain = 1024,
+ .pi_center_integrator_gain = 1024,
+ .pi_lower_integrator_gain = 2048,
+ .setpoint_offset = 0, /* CAL+0x52 - CAL+0x54 — populated from ROM */
+};
+
+const pwm_flash_t pwm_flash_default = {
+ .max_error_x = {0},
+ .max_error_y = {0},
+ .max_error_count = 1,
+ .max_error_input_addr = 0,
+ .request_upper_limit = 0,
+ .large_pos_error_thresh = 0x200,
+ .large_neg_error_thresh = (int16_t)0xFE00,
+ .inj_qty_demand_thresh = 0,
+ .rpm_breakpoints = {0},
+ .rpm_values = {0},
+ .shape_scale_src = 24,
+ .period_max_limit = 0x8000,
+ .period_min_limit = 0x6000,
+ .pwm_y_table = NULL,
+ .shape_y_table = NULL,
+};
+
+/* ROM-extracted pwm_cal_rom, pwm_flash_rom, pwm_submap_descrs[], and all
+ * Y-table / submap-x static payloads live in cal_tables_rom.{h,c} — both
+ * auto-generated by tools/extract_calibration.py --target compact. Link
+ * cal_tables_rom.c alongside pwm.c to get them. */
diff --git a/Core/Advance_Control/pwm_004002.h b/Core/Advance_Control/pwm_004002.h
new file mode 100644
index 0000000..1085005
--- /dev/null
+++ b/Core/Advance_Control/pwm_004002.h
@@ -0,0 +1,319 @@
+/**
+ * @file pwm.h
+ * @brief Compact single-header API for the VP44 PWM solenoid controller.
+ *
+ * Public API is just two functions:
+ * pwm_init() — one-time setup; caches cal/flash/getters into rt
+ * pwm_service() — per-cycle update; pulls external inputs via getters,
+ * runs the 6-stage control pipeline, writes rt outputs
+ *
+ * External inputs (sensors, CAN, HW) arrive exclusively through the
+ * pwm_input_getters_t vtable. Each getter returns a value in native PWM
+ * units — put any unit/scale conversion inside your getter body.
+ *
+ * Every arithmetic choice (MUL vs MULU, SHRA vs SHR, JGE vs JC) mirrors the
+ * MCS-96 assembly; see src/ for per-stage commentary + disassembly address
+ * cross-references.
+ */
+#ifndef PWM_H
+#define PWM_H
+
+#include
+#include
+#include
+
+/* ── MCS-96 arithmetic primitives ───────────────────────────────────── */
+#define MUL_S16(a, b) ((int32_t)(int16_t)(a) * (int32_t)(int16_t)(b))
+#define MULU_U16(a, b) ((uint32_t)(uint16_t)(a) * (uint32_t)(uint16_t)(b))
+#define CLAMP(v, lo, hi) ((v) < (lo) ? (lo) : (v) > (hi) ? (hi) : (v))
+#define INTEG_HI(s) ((int16_t)(((s) >> 16) & 0xFFFF))
+#define INTEG_LO(s) ((uint16_t)((s) & 0xFFFF))
+
+/** Portable signed right shift — guarantees sign extension (SHRA/SHRAL). */
+static inline int16_t shra16(int16_t v, unsigned n) {
+ if (!n) return v;
+ uint16_t u = (uint16_t)v, sign = (u >> 15) & 1u;
+ uint16_t m = sign ? (uint16_t)(((uint16_t)~0u) << (16u - n)) : 0u;
+ return (int16_t)((u >> n) | m);
+}
+static inline int32_t shra32(int32_t v, unsigned n) {
+ if (!n) return v;
+ uint32_t u = (uint32_t)v, sign = (u >> 31) & 1u;
+ uint32_t m = sign ? (~(uint32_t)0u << (32u - n)) : 0u;
+ return (int32_t)((u >> n) | m);
+}
+
+/* ── Mode flag bit definitions (address 0x028c) ─────────────────────── */
+#define MODE_FIRST_CALL 0x01 /* open→closed transition */
+#define MODE_OUTER_REGION 0x02 /* error outside center region */
+#define MODE_NEG_SAT 0x04 /* lower saturation */
+#define MODE_POS_SAT 0x08 /* upper saturation */
+#define MODE_LARGE_POS 0x10 /* large positive error */
+#define MODE_LARGE_NEG 0x20 /* large negative error */
+#define MODE_PREV_MASK 0xC0 /* shadow of prev cycle bits 4-5 */
+
+#define INTERP_MAX_ENTRIES 16
+
+/* ══════════════════════════════════════════════════════════════════════
+ * 1D INTERPOLATION SLOT
+ * Decoded by eval_submap once per input, then consumed by combine_submaps
+ * (bilinear, takes two slots) or refine_submap (1D, takes one slot).
+ * Four int16s; no cycle-to-cycle state.
+ * ══════════════════════════════════════════════════════════════════════ */
+typedef struct pwm_interp_slot {
+ int16_t row_stride; /* count * 2 — Y-table row byte-stride (A-axis width) */
+ int16_t x_interval; /* x[k-1] - x[k] — denominator (dx between breaks) */
+ int16_t x_offset; /* input - x[k] — numerator (distance from lower break) */
+ int16_t y_byte_off; /* k * 2 — Y-table byte-offset at break k */
+} pwm_interp_slot_t;
+
+/* ══════════════════════════════════════════════════════════════════════
+ * EXTERNAL INPUTS
+ * Populated each cycle by pwm_service via the getter vtable. All values
+ * must be in native PWM units — do any unit/scale conversion inside your
+ * getter. Address comments are the MCS-96 RAM location of the original
+ * variable in the ROM (for cross-reference with disassembly only).
+ * ══════════════════════════════════════════════════════════════════════ */
+typedef struct pwm_inputs {
+ int16_t ckp_in; /* 0x02f8 — CKP-derived sensed position */
+ uint16_t rpm; /* 0x0040 — engine RPM */
+ int16_t angle_dec_cmd; /* 0x0042 — CAN: angle-decrease command (sp2_B submap input) */
+ int16_t inj_qty_demand; /* 0x0044 — CAN: injection quantity demand (sp0_B + PI large-neg threshold) */
+ int16_t b_fb_kw; /* 0x02b4 — CAN: B_FB_KW plunger feedback demand (setpoint baseline) */
+ int16_t state_130; /* 0x0130 — CAN: open/closed-loop discriminant (< 0 forces open-loop) */
+ uint16_t supply_voltage; /* 0x0142 — battery / supply voltage (shape_eval submap input) */
+ int16_t temperature; /* 0x0146 — temperature (sp1_B submap input) */
+ /* NOTE: pwm_B's submap input_var=0x0046 in the ROM. That address is RW46,
+ * the register into which finalize_angle_request writes active_request
+ * at exit (disasm 0x73f4). It is NOT an external input; our port binds
+ * that descriptor's input_ptr directly to rt->active_request. */
+} pwm_inputs_t;
+
+/** Getter vtable. Each callback reads one external signal from your system
+ * and returns it in native PWM units. `ctx` is opaque to the PWM library
+ * and is passed back unchanged so your getters can reach their own state.
+ * All callbacks are required (no NULL entries).
+ */
+typedef struct pwm_input_getters {
+ int16_t (*ckp_in) (void *ctx);
+ uint16_t (*rpm) (void *ctx);
+ int16_t (*angle_dec_cmd) (void *ctx);
+ int16_t (*inj_qty_demand)(void *ctx);
+ int16_t (*b_fb_kw) (void *ctx);
+ int16_t (*state_130) (void *ctx);
+ uint16_t (*supply_voltage)(void *ctx);
+ int16_t (*temperature) (void *ctx);
+ void *ctx; /* user data; forwarded to every getter */
+} pwm_input_getters_t;
+
+/* Forward decls for the vtables */
+typedef struct pwm_calibration pwm_calibration_t;
+typedef struct pwm_flash pwm_flash_t;
+typedef struct pwm_submap_descr pwm_submap_descr_t;
+
+/* ══════════════════════════════════════════════════════════════════════
+ * RUNTIME STATE
+ * Holds everything that persists cycle-to-cycle plus per-cycle working
+ * values. External inputs live in rt->inputs (populated each cycle).
+ * ══════════════════════════════════════════════════════════════════════ */
+typedef struct pwm_runtime {
+ /* External inputs — refreshed each cycle by pwm_service from getters */
+ pwm_inputs_t inputs;
+
+ /* Async / mixed-direction flags.
+ * reset_flag — set by user/ISR on reset edge; cleared by supervisor.
+ * system_flags_110 — bit 5 (0x20): external "force open-loop" request;
+ * bit 0 (0x01): internal latch set by fast_recovery,
+ * cleared by PI when error_persist_counter hits zero.
+ */
+ uint8_t reset_flag; /* 0x002e */
+ uint8_t system_flags_110; /* 0x0110 */
+
+ /* ── Setpoint pipeline ───────────────────────────────────────────── */
+ int16_t compensation_angle; /* 0x02b6 — sum of 3 submap-pair outputs */
+ int16_t pre_offset_target; /* 0x012a — (b_fb_kw + compensation_angle) >> 1 */
+ int16_t setpoint_offset; /* 0x0150 — user-supplied setpoint bias */
+ int16_t target; /* RW5E — final setpoint (clamped) */
+ pwm_interp_slot_t setpoint_slot_x; /* 0x02b8 — per-call eval → combine */
+ pwm_interp_slot_t setpoint_slot_y; /* 0x02c0 */
+
+ /* ── Supervisor ──────────────────────────────────────────────────── */
+ int16_t cl_enable_counter; /* 0x0340 */
+ int16_t cl_error_delta; /* 0x0342 */
+ int16_t max_cl_error; /* 0x02f2 */
+ int16_t supervisor_state_17e; /* 0x017e — CL-correction accumulator */
+
+ /* ── PI controller ───────────────────────────────────────────────── */
+ int16_t angle_error; /* 0x0278 */
+ int16_t angle_error_shaped; /* 0x0276 */
+ uint8_t mode_flags; /* 0x028c */
+ int32_t integrator_state; /* 0x0288/028a */
+ int16_t integrator_gain; /* 0x0286 */
+ int16_t active_request; /* 0x0274 — final angle request */
+ int16_t estimated_angle; /* 0x02cc */
+ int16_t angle_offset; /* 0x017c */
+ int16_t cl_correction_raw; /* 0x0176 */
+ int16_t recovery_counter; /* 0x0118 */
+ int16_t recovery_count_threshold;/* 0x027a */
+ int16_t error_persist_counter; /* 0x027c */
+ int16_t first_call_gain; /* 0x02ec */
+ int16_t pos_error_normalizer; /* 0x02ee */
+ int16_t neg_error_normalizer; /* 0x02f0 */
+ int16_t min_rpm_openloop; /* 0x015c */
+
+ /* PI shaping (populated from CAL by pwm_init) */
+ int16_t upper_breakpoint, lower_breakpoint;
+ int16_t center_slope, upper_outer_slope, lower_outer_slope;
+ int16_t upper_integrator_gain, center_integrator_gain, lower_integrator_gain;
+
+ /* ── PWM output ──────────────────────────────────────────────────── */
+ uint16_t pwm_duty; /* 0x02d2 */
+ uint16_t pwm_period; /* 0x0330 */
+ uint16_t pwm_on_time, pwm_off_time; /* 0x02ce / 0x02d0 — HW compare regs */
+ uint8_t pwm_status_flag; /* 0x00d1 */
+ int16_t shape_scale; /* 0x0338 */
+ int16_t shape_intermediate; /* 0x0332 */
+ int16_t shape_output; /* 0x033a */
+ uint16_t pwm_min, pwm_max; /* 0x0158 / 0x015a */
+ pwm_interp_slot_t pwm_slot_x; /* 0x0290 — pwm_A eval → combine */
+ pwm_interp_slot_t pwm_slot_y; /* 0x0298 — pwm_B eval → combine */
+
+ /* ── Bindings (set once by pwm_init, consumed by pwm_service) ───── */
+ const pwm_calibration_t *bound_cal;
+ const pwm_flash_t *bound_flash;
+ const pwm_input_getters_t *bound_getters;
+} pwm_runtime_t;
+
+/* ── Calibration (TABLE[RWA4]+offset) ───────────────────────────────── */
+struct pwm_calibration {
+ int16_t target_minimum; /* CAL+0x11E */
+ /* Y-table pointers for each submap pair. Each field originally held a
+ * 16-bit ROM address (CAL+0x184/186/188). [0]=RPM×InjQty, [1]=RPM×Temp,
+ * [2]=RPM×AngleDec — see src/submap_inputvars.txt */
+ const int16_t *y_pair[3]; /* CAL+0x184/186/188 */
+ /* PI shaping (CAL+0x00FE…0x0116) */
+ int16_t pi_upper_breakpoint, pi_lower_breakpoint;
+ int16_t pi_center_slope, pi_upper_outer_slope, pi_lower_outer_slope;
+ int16_t pi_upper_integrator_gain, pi_center_integrator_gain, pi_lower_integrator_gain;
+
+ /* compute_closed_loop_correction tuning */
+ int16_t closed_loop_gain_const; /* CAL+0x0156 */
+
+ /* fast_recovery tuning */
+ int16_t error_persist_init_count; /* CAL+0x0108 */
+ int16_t recovery_rpm_threshold; /* CAL+0x011A */
+
+ /* Setpoint offset latched once at init by FUN_8643 (0x867d):
+ * RAM[0x0150] = CAL[0x52] - (CAL[0x54] + RAM[0x0430])
+ * RAM[0x0430] is observed as always 0 at runtime, so the effective
+ * constant is CAL[0x52] - CAL[0x54]. */
+ int16_t setpoint_offset; /* CAL+0x0052 - CAL+0x0054 (RAM 0x0150) */
+};
+
+/* ── Flash-resident tables ──────────────────────────────────────────── */
+struct pwm_flash {
+ int16_t max_error_x[INTERP_MAX_ENTRIES];
+ int16_t max_error_y[INTERP_MAX_ENTRIES];
+ uint16_t max_error_count;
+ int16_t max_error_input_addr;
+ int16_t request_upper_limit; /* FLASH:9734 */
+ int16_t large_pos_error_thresh; /* FLASH:971a */
+ int16_t large_neg_error_thresh; /* FLASH:971c */
+ int16_t inj_qty_demand_thresh; /* FLASH:9722 */
+ uint16_t rpm_breakpoints[8]; /* FLASH:96fe */
+ uint16_t rpm_values[8]; /* FLASH:970e */
+ int16_t shape_scale_src; /* FLASH:9710 */
+ uint16_t period_max_limit, period_min_limit; /* FLASH:96fa / 96fc */
+ const int16_t *pwm_y_table; /* FLASH:9768 — 2D Y-table */
+ const int16_t *shape_y_table; /* FLASH:9772 — 1D Y-table */
+};
+
+/* ── Submap descriptor (mirrors calibration-block layout) ───────────── */
+struct pwm_submap_descr {
+ uint16_t flags; /* +0 */
+ const int16_t *input_ptr; /* +2 (bound at runtime) */
+ uint16_t count; /* +4 */
+ const int16_t *x; /* +6 */
+ uint16_t input_addr; /* original address (for input_ptr binding) */
+};
+
+/** Bind submap descriptor input_ptr fields to the matching source in rt.
+ * Most descriptors point at fields in rt->inputs (external signals), but
+ * input_addr 0x0046 resolves to rt->active_request — in the ROM that RAM
+ * address is RW46, which finalize_angle_request writes with the PI
+ * output at its exit (disasm 0x73f4), so the pwm_B submap sees the
+ * latest active_request when pwm_output_compute runs.
+ * Recognised input_addr values: 0x0040, 0x0042, 0x0044, 0x0046, 0x0142,
+ * 0x0146. Others are bound to NULL. */
+static inline void pwm_bind_submap_inputs(pwm_runtime_t *rt,
+ pwm_submap_descr_t *descrs,
+ uint16_t n)
+{
+ for (uint16_t i = 0; i < n; i++) {
+ switch (descrs[i].input_addr) {
+ case 0x0040: descrs[i].input_ptr = (const int16_t *)&rt->inputs.rpm; break;
+ case 0x0042: descrs[i].input_ptr = &rt->inputs.angle_dec_cmd; break;
+ case 0x0044: descrs[i].input_ptr = &rt->inputs.inj_qty_demand; break;
+ case 0x0046: descrs[i].input_ptr = &rt->active_request; break;
+ case 0x0142: descrs[i].input_ptr = (const int16_t *)&rt->inputs.supply_voltage; break;
+ case 0x0146: descrs[i].input_ptr = &rt->inputs.temperature; break;
+ default: descrs[i].input_ptr = NULL; break;
+ }
+ }
+}
+
+/* ══════════════════════════════════════════════════════════════════════
+ * PUBLIC API
+ * ══════════════════════════════════════════════════════════════════════ */
+
+/** One-time setup. Zero-inits internal state, applies PI shaping from
+ * cal, and caches cal/flash/getters into rt for later use by pwm_service.
+ * None of the pointer arguments may be NULL. */
+void pwm_init(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal,
+ const pwm_flash_t *flash,
+ const pwm_input_getters_t *getters);
+
+/** Per-cycle update. Calls each getter to populate rt->inputs, then
+ * runs the 6-stage control pipeline:
+ * 1. max_cl_error lookup
+ * 2. setpoint computation
+ * 3. supervisor (error + CL-enable + clamping)
+ * 4. closed-loop correction + angle estimate
+ * 5. PI controller
+ * 6. PWM duty + HW register split
+ * Outputs are written to rt (pwm_on_time, pwm_off_time, pwm_period, etc).
+ */
+void pwm_service(pwm_runtime_t *rt);
+
+/**
+ * Notify the controller of a CKP (crankshaft position) pulse edge.
+ *
+ * ROM equivalent: EPA/CKP ISR at 0x877f, which re-latches ckp_in and sets
+ * reset_flag=1 as its terminal step. In this port, ckp_in is pulled from
+ * the getter on each pwm_service() call, so this hook is responsible only
+ * for the reset_flag edge. The supervisor consumes and clears the flag on
+ * the next pwm_service(), which zeroes supervisor_state_17e so the CL
+ * accumulator does not walk off between pulses.
+ *
+ * Call from your CKP edge ISR (or equivalent polling point). Safe to call
+ * between pwm_service() invocations; do NOT call concurrently with
+ * pwm_service(). Byte store is atomic on all mainstream targets — no
+ * additional locking required.
+ *
+ * Rate on VP44: 4 pulses per engine revolution. At 1200 rpm that is
+ * ~80 Hz / ~12 scheduler ticks at the 1 kHz control cadence.
+ */
+static inline void pwm_ckp_isr(pwm_runtime_t *rt) { rt->reset_flag = 1; }
+
+/** Utility: 1D descending-X piecewise-linear lookup (helper_from_cal). */
+int16_t pwm_interp_lookup(const int16_t *x, const int16_t *y,
+ uint16_t n, int16_t in);
+
+/* ── Default & ROM-extracted data ───────────────────────────────────── */
+extern const pwm_calibration_t pwm_cal_default;
+extern const pwm_flash_t pwm_flash_default;
+extern const pwm_calibration_t pwm_cal_rom;
+extern const pwm_flash_t pwm_flash_rom;
+
+#endif /* PWM_H */
diff --git a/Core/Advance_Control/pwm_504012.c b/Core/Advance_Control/pwm_504012.c
new file mode 100644
index 0000000..30472b9
--- /dev/null
+++ b/Core/Advance_Control/pwm_504012.c
@@ -0,0 +1,739 @@
+/**
+ * @file pwm.c (families/t06211/compact_src)
+ * @brief Compact single-file implementation of the t06211 PWM control
+ * pipeline. Merges the 10 per-module files from
+ * families/t06211/src/ into one translation unit.
+ *
+ * Pipeline (t06211 FUN_77b3 @ 0x77b3):
+ * 1. setpoint (FUN_7168) — single-submap RPM-indexed interp
+ * 2. supervisor (FUN_7beb) — reset + counter + error + clamp
+ * 3. publish_cl (FUN_7cd8) — calls cl_correction, publishes est_angle
+ * 4. pi_controller (FUN_67c4) — open/closed-loop branch
+ * 5. pwm_output (FUN_5314) — eval+eval+combine+saturate+HW shadow
+ *
+ * Per-block address citations follow the families/t06211/src/ per-module
+ * ports verbatim; see those files for expanded commentary.
+ */
+#include "pwm.h"
+
+/* Forward decls for file-static helpers. */
+static void s_eval_submap(const pwm_submap_descr_t *d,
+ pwm_interp_slot_t *slot);
+static int16_t s_combine(const int16_t *y_base,
+ const pwm_interp_slot_t *sa,
+ const pwm_interp_slot_t *sb);
+static int16_t s_refine(const int16_t *y_base,
+ const pwm_interp_slot_t *slot);
+
+static void s_setpoint (pwm_runtime_t *rt);
+static void s_supervisor (pwm_runtime_t *rt);
+static void s_cl_correct (pwm_runtime_t *rt,
+ const pwm_calibration_t *cal);
+static void s_publish_cl (pwm_runtime_t *rt,
+ const pwm_calibration_t *cal);
+static void s_pi_update (pwm_runtime_t *rt,
+ const pwm_calibration_t *cal);
+static void s_pwm_output (pwm_runtime_t *rt,
+ const pwm_calibration_t *cal);
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Init / binding
+ * ═════════════════════════════════════════════════════════════════════ */
+
+static void runtime_reset(pwm_runtime_t *rt)
+{
+ memset(rt, 0, sizeof(*rt));
+ /* Normalizers are ROM-resident; fallback defaults for safety. */
+ rt->pos_error_normalizer = 500;
+ rt->neg_error_normalizer = 650;
+
+ /* PI Block-4 error-window bounds — mirror the one-shot boot init
+ * done by FUN_76aa in the real ROM. Scratch bytes at RAM[0x0414] /
+ * [0x0416] have no writers anywhere in the ROM (EEPROM trim bytes,
+ * default 0), so the bounds resolve to cal+0x10A (=+853) and
+ * cal+0x10C (=-853). See docs/open-questions.md §2. */
+ rt->pi_error_bound_pos = +853;
+ rt->pi_error_bound_neg = -853;
+
+ /* PI compensation/integrator scalars — defaults from FUN_76aa
+ * boot init. cal+0x118=+480, cal+0x11A=+256, cal+0x11C=6 (byte). */
+ rt->pi_post_scale_454 = 480;
+ rt->pi_integ_step_456 = 256;
+ rt->pi_integ_gain_330 = 6;
+
+ /* CAN-setpoint defaults. setpoint_offset_150 mirrors family-1's
+ * pwm_init copy pattern (compact_src/pwm.c:66) — it's a cal-derived
+ * static bias, not a runtime value. */
+ rt->rw42_state = 0;
+ rt->can_raw_b_fb_kw = 0;
+ rt->can_aux_12e = 0;
+ rt->target_5e = 0;
+ rt->target_336 = 0;
+}
+
+static void apply_cal(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal)
+{
+ /* Mirrors family-1's pwm_init copy of cal→runtime statics. */
+ rt->setpoint_offset_150 = cal->setpoint_offset;
+}
+
+void pwm_init(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal,
+ const pwm_flash_t *flash,
+ const pwm_input_getters_t *getters)
+{
+ (void)flash; /* family-1 API parity; t06211 keeps Y-tables in cal */
+ runtime_reset(rt);
+ rt->bound_cal = cal;
+ rt->bound_getters = getters;
+ apply_cal(rt, cal);
+ pwm_bind_submap_inputs(rt, pwm_submap_descrs, PWM_SUBMAP_COUNT);
+}
+
+static void read_inputs(pwm_runtime_t *rt)
+{
+ const pwm_input_getters_t *g = rt->bound_getters;
+ void *ctx = g->ctx;
+ rt->inputs.ckp_in = g->ckp_in (ctx);
+ rt->inputs.rpm = g->rpm (ctx);
+ rt->inputs.angle_dec_cmd = g->angle_dec_cmd (ctx); /* accepted; unused */
+ rt->inputs.inj_qty_demand = g->inj_qty_demand(ctx);
+ rt->inputs.b_fb_kw = g->b_fb_kw (ctx);
+ rt->inputs.state_130 = g->state_130 (ctx);
+ rt->inputs.supply_voltage = g->supply_voltage(ctx);
+ rt->inputs.temperature = g->temperature (ctx); /* accepted; unused */
+
+ /* The b_fb_kw getter result drives the CAN-decoded setpoint chain
+ * (FUN_64c3). Mirrors the real ROM where RAM[0x12c] is written by
+ * the CAN parser before FUN_64c3 runs. */
+ rt->can_raw_b_fb_kw = rt->inputs.b_fb_kw;
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Interpolation — FUN_7168 (0x7168-0x71d7), fingerprint #3
+ * Raw helper; also drives the setpoint stage via descriptor.
+ * ═════════════════════════════════════════════════════════════════════ */
+
+int16_t pwm_interp_lookup(const int16_t *x, const int16_t *y,
+ uint16_t n, int16_t in)
+{
+ if (in >= x[0]) return y[0];
+ if (in <= x[n - 1u]) return y[n - 1u];
+
+ uint16_t k = 1u;
+ while (k < n && in < x[k]) { k++; }
+
+ int16_t x_k = x[k];
+ int16_t num = (int16_t)(in - x_k);
+ int16_t dx = (int16_t)(x_k - x[k - 1u]);
+ int16_t dy = (int16_t)(y[k] - y[k - 1u]);
+
+ int32_t prod = MUL_S16(num, dy);
+ int16_t quot = (int16_t)(prod / (int32_t)dx);
+ /* Disasm 0x71c5: ADD RW1C, -0x2[RW20]. After the disasm's pointer
+ * advance to the y[] half (0x71b2 ADD RW20, RW1E), RW20 points one
+ * past the breakpoint where the search settled, so -0x2[RW20] is
+ * y at that breakpoint — which is y[k] in C's k-naming (k = first
+ * index where in >= x[k] given descending x). Earlier this returned
+ * y[k-1], producing wrong-polarity setpoints (e.g. target_336 = 1962
+ * instead of 1195 at rpm=3355). */
+ return (int16_t)(quot + y[k]);
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Submap eval / bilinear combine / refine — t06211 FUN_6fb8 / FUN_7035 /
+ * FUN_7014. Scratch layout matches the family-1 ROM convention.
+ * ═════════════════════════════════════════════════════════════════════ */
+
+static void s_eval_submap(const pwm_submap_descr_t *d,
+ pwm_interp_slot_t *slot)
+{
+ int16_t input = d->input_ptr ? *d->input_ptr : 0;
+ slot->row_stride = (int16_t)(d->count * 2u);
+
+ if (d->count == 0 || d->x == NULL) {
+ slot->x_interval = 2;
+ slot->x_offset = 2;
+ slot->y_byte_off = 2;
+ return;
+ }
+
+ uint16_t k;
+ for (k = 1u; k < d->count; k++) {
+ if (input >= d->x[k]) break;
+ }
+
+ if (k == 1u && input >= d->x[0]) {
+ /* Upper-clamp sentinel [_, 2, 2, 2] */
+ slot->x_interval = 2;
+ slot->x_offset = 2;
+ slot->y_byte_off = 2;
+ return;
+ }
+ if (k >= d->count) { k = (uint16_t)(d->count - 1u); }
+
+ slot->x_interval = (int16_t)(d->x[k - 1u] - d->x[k]);
+ slot->x_offset = (int16_t)(input - d->x[k]);
+ slot->y_byte_off = (int16_t)(k * 2u);
+}
+
+static int16_t s_combine(const int16_t *y_base,
+ const pwm_interp_slot_t *sa,
+ const pwm_interp_slot_t *sb)
+{
+ if (!y_base || !sa || !sb) return 0;
+
+ int16_t den_a = sa->x_interval ? sa->x_interval : 1;
+ int16_t den_b = sb->x_interval ? sb->x_interval : 1;
+
+ int32_t row_off = MUL_S16(sb->y_byte_off, sa->row_stride) / 2;
+ const uint8_t *yp_b = (const uint8_t *)y_base + row_off + sa->y_byte_off;
+ const int16_t *yp = (const int16_t *)yp_b;
+
+ int16_t y_here = *yp;
+ int16_t y_prev_a = *(const int16_t *)(yp_b - 2);
+ int32_t diff_a = MUL_S16((int16_t)(y_prev_a - y_here), sa->x_offset);
+ int16_t rowB = (int16_t)(y_here + (int32_t)(diff_a / (int32_t)den_a));
+
+ const uint8_t *yp_b_prev = yp_b - sa->row_stride;
+ const int16_t *yp_prev = (const int16_t *)yp_b_prev;
+ int16_t y_here_pb = *yp_prev;
+ int16_t y_prev_a_pb = *(const int16_t *)(yp_b_prev - 2);
+ int32_t diff_a_pb = MUL_S16((int16_t)(y_prev_a_pb - y_here_pb), sa->x_offset);
+ int16_t rowBp = (int16_t)(y_here_pb + (int32_t)(diff_a_pb / (int32_t)den_a));
+
+ int32_t diff_b = MUL_S16((int16_t)(rowBp - rowB), sb->x_offset);
+ return (int16_t)(rowB + (int32_t)(diff_b / (int32_t)den_b));
+}
+
+static int16_t s_refine(const int16_t *y_base,
+ const pwm_interp_slot_t *slot)
+{
+ if (!y_base || !slot) return 0;
+ int16_t den = slot->x_interval ? slot->x_interval : 1;
+
+ const uint8_t *yp_b = (const uint8_t *)y_base + slot->y_byte_off;
+ const int16_t *yp = (const int16_t *)yp_b;
+ int16_t y_here = *yp;
+ int16_t y_prev_a = *(const int16_t *)(yp_b - 2);
+ int32_t diff = MUL_S16((int16_t)(y_prev_a - y_here), slot->x_offset);
+ return (int16_t)(y_here + (int32_t)(diff / (int32_t)den));
+}
+
+/* Descriptor-driven wrapper used only by the setpoint stage. */
+static int16_t s_interp_descr(const pwm_submap_descr_t *d,
+ const int16_t *y_array)
+{
+ int16_t input = d->input_ptr ? *d->input_ptr : 0;
+ return pwm_interp_lookup(d->x, y_array, d->count, input);
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Stage 1 — Setpoint (FUN_77b3:77b3-77c7 + FUN_7168)
+ * ═════════════════════════════════════════════════════════════════════ */
+
+static void s_setpoint(pwm_runtime_t *rt)
+{
+ const pwm_submap_descr_t *d =
+ &pwm_submap_descrs[PWM_SUBMAP_SETPOINT_INTERP];
+ const int16_t *y = pwm_submap_y_of(PWM_SUBMAP_SETPOINT_INTERP);
+ rt->target_336 = s_interp_descr(d, y);
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Stage 1b — CAN-decoded setpoint (FUN_64c3 @ 0x64c3-0x650a)
+ * Real ROM call site: FUN_6192:0x61cc (CAN parser dispatcher).
+ * For the C model, invoked from pwm_service after read_inputs so the
+ * harness's per-cycle b_fb_kw value flows into target before PI runs.
+ * Skip the RE7 gate and error path (sim trusts caller).
+ * ═════════════════════════════════════════════════════════════════════ */
+
+static void s_setpoint_can_decode(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal)
+{
+ int16_t raw = rt->can_raw_b_fb_kw;
+ /* [0x64ca-0x64d6] Bounds check */
+ if (raw > cal->b_fb_kw_upper_bound) return;
+ if (raw < cal->b_fb_kw_lower_bound) return;
+
+ /* [0x64dd] half = raw >> 1 (SHRA — sign-extending) */
+ int16_t half = shra16(raw, 1);
+ rt->can_half_12a = half;
+
+ /* [0x64e5-0x64ea] result = half + RAM[0x150] + RW42 */
+ int16_t result = (int16_t)(half + rt->setpoint_offset_150 + rt->rw42_state);
+
+ /* [0x64ed-0x64f9] Lower clamp */
+ if (result < cal->target_5e_min_clamp) {
+ result = cal->target_5e_min_clamp;
+ }
+ rt->target_5e = result;
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Stage 2 — Supervisor (FUN_7beb @ 0x7beb-0x7c41)
+ * ═════════════════════════════════════════════════════════════════════ */
+
+static void s_supervisor(pwm_runtime_t *rt)
+{
+ /* Reset-flag branch (0x7beb-0x7c0c) */
+ if (rt->reset_flag == 1u) {
+ rt->reset_flag = 0u;
+ rt->cl_enable_counter = 0u;
+ rt->supervisor_state_17e = 0;
+ rt->b_fb_kw_baseline = rt->inputs.b_fb_kw;
+ } else {
+ /* Counter tick (0x7c11-0x7c1b) */
+ rt->cl_enable_counter = (uint16_t)(rt->cl_enable_counter + 1u);
+ }
+
+ /* Error compute (0x7c20-0x7c2b)
+ * Disasm: SUB RW1C, RW5E, DAT_02f8 — primary setpoint is target. */
+ int16_t error = (int16_t)(rt->target_5e - rt->inputs.ckp_in);
+ error = (int16_t)(error + rt->compensation_angle);
+ rt->angle_error_raw = error;
+
+ /* Ceiling clamp (0x7c30-0x7c41)
+ * Disasm: CMP RW1C, DAT_0336 — clamp uses RPM-derived ceiling. */
+ if (error > rt->target_336) {
+ rt->angle_error_raw = rt->target_336;
+ }
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Stage 3a — Closed-loop correction (FUN_5f1f @ 0x5f1f-0x5f66)
+ * Fingerprint #10 bit-for-bit match to family-1 algorithm.
+ * ═════════════════════════════════════════════════════════════════════ */
+
+static void s_cl_correct(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal)
+{
+ int16_t correction;
+
+ /* Gate on low byte of cl_enable_counter (0x5f1f-0x5f24). */
+ if ((rt->cl_enable_counter & 0xFFu) == 0u) {
+ correction = 0;
+ } else {
+ int16_t normalizer = (rt->angle_error_raw > 0)
+ ? rt->pos_error_normalizer
+ : rt->neg_error_normalizer;
+
+ int32_t product = MUL_S16(rt->angle_error_raw, cal->closed_loop_gain_const);
+ product = (int32_t)((uint32_t)product << 4);
+ correction = (normalizer != 0)
+ ? (int16_t)(product / (int32_t)normalizer)
+ : (int16_t)0;
+ }
+
+ rt->cl_correction_raw = correction;
+ rt->supervisor_state_17e = (int16_t)(rt->supervisor_state_17e + correction);
+ rt->angle_offset = shra16(rt->supervisor_state_17e, 4u);
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Stage 3 — Publish CL (FUN_7cd8 @ 0x7cd8-0x7cea)
+ * ═════════════════════════════════════════════════════════════════════ */
+
+static void s_publish_cl(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal)
+{
+ s_cl_correct(rt, cal);
+ rt->estimated_angle = (int16_t)(rt->inputs.ckp_in + rt->angle_offset);
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Stage 4 — PI controller (FUN_67c4 @ 0x67c4 + FUN_66a8 + FUN_672b)
+ * ═════════════════════════════════════════════════════════════════════ */
+
+/* FUN_66a8 — saturation-latch recovery handler. Role-equivalent to the
+ * default family's `s_recovery` / `fast_recovery`; same name across
+ * variants for cross-family alignment. Disasm/source mapping (audit
+ * table — keep the cooldown gate and gated-increment branches in this
+ * exact order):
+ * 66ad-66b2 CMPB pi_shape_flag, pi_flag_c6 ; JNE LAB_671d
+ * 66b9-66be CMP pi_state_118, [0x02c0]=cal+0x112 ; JNC LAB_66e6
+ * JNC ⇒ counter < threshold ⇒ gated-increment
+ * fall-through ⇒ counter ≥ threshold ⇒ latch+reset
+ * 66c0-66e5 latch bit0 ; counter = 0 ; pi_state_c2 ← cal+0x114
+ * 66e6-66f4 CMP rpm, [cal+0x11E] ; JLE exit
+ * 66f6-6705 JBS bit4 / JBS bit5 — exit
+ * 6706-670b CMP ZR, pi_state_c2 ; JNE exit
+ * 670d-6717 pi_state_118 += 1
+ * 671d-6725 JBS bit0 exit ; pi_state_118 = 0
+ */
+static void s_recovery(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal)
+{
+ if (rt->pi_shape_flag != rt->pi_flag_c6) {
+ if ((rt->system_flags_110 & 0x01u) == 0u) {
+ rt->pi_state_118 = 0;
+ }
+ return;
+ }
+ /* Counter ≥ threshold → latch+reset. Threshold is boot-cached at
+ * RAM[0x02c0] from cal+0x112 by FUN_6b7b:0x6bd4. */
+ if ((uint16_t)rt->pi_state_118 >= (uint16_t)cal->pi_sat_count_threshold) {
+ rt->system_flags_110 = (uint8_t)(rt->system_flags_110 | 0x01u);
+ rt->pi_state_118 = 0;
+ rt->pi_state_c2 = cal->error_thresh_114;
+ return;
+ }
+ /* Gated increment: rpm > rpm_threshold_11E required. */
+ if ((int16_t)rt->inputs.rpm <= cal->rpm_threshold_11E) return;
+ /* Bits 4 or 5 of system_flags abort. */
+ if ((rt->system_flags_110 & 0x30u) != 0u) return;
+ /* c2 cooldown gate — counter advances only after c2 has decayed to 0. */
+ if (rt->pi_state_c2 != 0) return;
+ rt->pi_state_118 = (int16_t)(rt->pi_state_118 + 1);
+}
+
+/* FUN_7c85 @ 0x7c85-0x7cbf — PI integrator step. Anti-windup gates
+ * the update direction against pi_flag_c7 (0=in-range, 1=clamped low,
+ * 2=clamped high). Updates the {pi_b4_state:pi_b6_state} 32-bit pair.
+ * Disasm:
+ * 7c85 LD RW1C, err
+ * 7c8a MUL RL1C, [0x0456] ; signed 32-bit, RL1C = err*step
+ * 7c90 SHLL RL1C, #0x4 ; RL1C <<= 4
+ * 7c93 JGE LAB_7ca1 ; if signed result >= 0
+ * 7c95-7c9d if pi_flag_c7==1 RET ; (clamped-low → don't push more negative)
+ * 7c9f SJMP LAB_7cab ; else update
+ * LAB_7ca1: if pi_flag_c7==2 RET ; (clamped-high → don't push more positive)
+ * LAB_7cab: integrator += RL1C ; ADD lo, ADDC hi
+ */
+static void s_pi_integrator_step(pwm_runtime_t *rt)
+{
+ int32_t prod = MUL_S16(rt->angle_error_pi, rt->pi_integ_step_456);
+ int32_t step = (int32_t)((uint32_t)prod << 4);
+
+ if (step >= 0) {
+ if (rt->pi_flag_c7 == 0x02) return; /* anti-windup: clamped-high */
+ } else {
+ if (rt->pi_flag_c7 == 0x01) return; /* anti-windup: clamped-low */
+ }
+
+ /* Combine {hi:lo} as int32 -> add step -> split back. */
+ int32_t accum = ((int32_t)(int16_t)rt->pi_b4_state << 16)
+ | (uint16_t)rt->pi_b6_state;
+ accum += step;
+ rt->pi_b6_state = (int16_t)(accum & 0xFFFF);
+ rt->pi_b4_state = (int16_t)((accum >> 16) & 0xFFFF);
+}
+
+/* FUN_672b @ 0x672b-0x67c3 — PI compensation + final clamp.
+ * Two arms decided by (pi_flag_338==1 || pi_open_loop_flag==0):
+ * "fresh" → reset pi_b4_state from a new (err*pi_gain>>4)+ckp formula
+ * "settled" → step the integrator via FUN_7c85
+ * Then compute compensation_angle = (err * pi_post_scale_454) >> 8
+ * and active_request = comp + pi_b4_state, clamped to [pi_low_clamp, target].
+ */
+static void s_pi_compensation(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal)
+{
+ int16_t error = rt->angle_error_pi;
+
+ /* [0x672b-0x673a] Branch select. */
+ bool fresh = (rt->pi_flag_338 == 0x01) || (rt->pi_open_loop_flag == 0u);
+
+ if (fresh) {
+ /* [LAB_673c 0x673c-0x6762] Reset pi_b4_state. Disasm:
+ * STB ZRlo, DAT_0338 ; pi_flag_338 = 0
+ * LD RW1C, err
+ * EXT RL1C ; sign-extend to 32-bit
+ * MUL RL1C, [0x0330] ; signed: RL1C = err32 * pi_gain
+ * SHRA RW1C, #0x4 ; LOW WORD shifted (high word discarded)
+ * ADD RW1C, RW1C, ckp_in
+ * ST RW1C, DAT_02b4 ; pi_b4_state = result
+ */
+ rt->pi_flag_338 = 0;
+ int32_t prod = MUL_S16(error, (int16_t)rt->pi_integ_gain_330);
+ int16_t scaled = shra16((int16_t)(prod & 0xFFFF), 4);
+ rt->pi_b4_state = (int16_t)(scaled + rt->inputs.ckp_in);
+ } else {
+ /* [LAB_6764 0x6764] LCALL FUN_7c85 — integrator step. */
+ s_pi_integrator_step(rt);
+ }
+
+ /* [LAB_6767 0x6767-0x6779] compensation_angle = (RL1C = post_scale*err) >> 8.
+ * Disasm:
+ * LD RW1C, [0x0454] ; post_scale (default +480)
+ * MUL RL1C, err ; RL1C = post_scale * err (signed)
+ * SHRAL RL1C, #0x8 ; arithmetic shift right 8 (32-bit)
+ * ST RW1C, DAT_02b8 ; compensation_angle = lo16
+ */
+ int32_t comp32 = MUL_S16(rt->pi_post_scale_454, error);
+ int16_t comp = (int16_t)(shra32(comp32, 8) & 0xFFFF);
+ rt->compensation_angle = comp;
+
+ /* [0x677a-0x677f] active_request = comp + pi_b4_state */
+ int16_t combined = (int16_t)(comp + rt->pi_b4_state);
+ rt->active_request = combined;
+
+ /* [0x6782-0x67a8] Lower clamp: if active_request < pi_low_clamp,
+ * pin to pi_low_clamp and set pi_flag_c7=1. */
+ if (rt->active_request < cal->pi_low_clamp) {
+ rt->active_request = cal->pi_low_clamp;
+ rt->pi_flag_c7 = 0x01;
+ return;
+ }
+
+ /* [LAB_67a9 0x67a9-0x67bd] Upper clamp: if active_request > target,
+ * pin to target and set pi_flag_c7=2. */
+ /* Disasm 0x67a9-0x67b0: CMP RW46, DAT_0336 — upper clamp uses
+ * the RPM-derived ceiling, not the CAN-decoded primary setpoint. */
+ if (rt->active_request > rt->target_336) {
+ rt->active_request = rt->target_336;
+ rt->pi_flag_c7 = 0x02;
+ return;
+ }
+
+ /* [LAB_67be 0x67be-0x67c3] In-range: pi_flag_c7 = 0. */
+ rt->pi_flag_c7 = 0;
+}
+
+static void s_pi_update(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal)
+{
+ /* PI CL-gate threshold = cached ROM[0x605c]. Disasm site:
+ * FUN_67c4:67d9 `CMP RW1C, DAT_605c` (literal 0x01A4 = 420). In the
+ * real ECU, state_130 is almost always negative (CAN inactive
+ * sentinel) which forces OL at line 67d4 before this RPM gate is
+ * reached. Lifted to cal->pi_cl_rpm_floor. */
+ int16_t rpm_floor = cal->pi_cl_rpm_floor;
+ bool open_loop = false;
+ if ((rt->system_flags_110 & 0x20u) != 0u) open_loop = true;
+ else if (rt->inputs.state_130 < 0) open_loop = true;
+ else if ((int16_t)rt->inputs.rpm < rpm_floor) open_loop = true;
+
+ if (open_loop) {
+ rt->pi_open_loop_flag = 0;
+ /* Disasm 0x67e5: LD RW46, RW5E — open-loop assigns the CAN-decoded
+ * primary setpoint, NOT the RPM-derived ceiling. */
+ rt->active_request = rt->target_5e;
+ /* cal+0x120 is a LOWER bound here (disasm: JLT clamps when
+ * active_request < limit). Same constant also serves as the
+ * Block-4 low clamp, hence `pi_low_clamp`. See
+ * families/t06211/src/pi_controller_t06211.c for the full-disasm
+ * annotation. */
+ if (rt->active_request < cal->pi_low_clamp) {
+ rt->active_request = cal->pi_low_clamp;
+ }
+ goto final_flag;
+ }
+
+ /* Disasm 0x680a: SUB RW1C, RW5E, DAT_02cc — CL error uses target. */
+ rt->angle_error_pi = (int16_t)(rt->target_5e - rt->estimated_angle);
+ int16_t err = rt->angle_error_pi;
+
+ if (err > cal->large_pos_error_thresh) {
+ rt->pi_shape_flag = 0;
+ s_recovery(rt, cal);
+ } else if (err < cal->large_neg_error_thresh) {
+ rt->pi_shape_flag = 0x01;
+ if (rt->inputs.inj_qty_demand <= cal->pi_thresh_116) {
+ rt->pi_state_118 = 0;
+ } else {
+ s_recovery(rt, cal);
+ }
+ } else {
+ rt->pi_shape_flag = 0x20;
+ if (rt->pi_state_c2 > 0) {
+ rt->pi_state_c2 = (int16_t)(rt->pi_state_c2 - 1);
+ }
+ if (rt->pi_state_c2 == 0) {
+ rt->system_flags_110 = (uint8_t)(rt->system_flags_110 & 0xFEu);
+ }
+ if (rt->pi_state_118 > 0) {
+ rt->pi_state_118 = (int16_t)(rt->pi_state_118 - 1);
+ }
+ }
+
+ /* ── Block 4: error-window decision [0x68b7-0x68ef] ──
+ * Strict > / < per disasm JLE 68c9 / JGE 68ed. Bounds at
+ * 0x0450/0x0452 are set once at boot by FUN_76aa — see
+ * docs/open-questions.md §2 closeout. */
+ if (err > rt->pi_error_bound_pos) {
+ rt->active_request = cal->pi_high_clamp;
+ rt->pi_flag_338 = 0x01;
+ } else if (err < rt->pi_error_bound_neg) {
+ rt->active_request = cal->pi_low_clamp;
+ rt->pi_flag_338 = 0x01;
+ } else {
+ s_pi_compensation(rt, cal);
+ }
+
+final_flag:
+ rt->pi_open_loop_flag = 0x01;
+ rt->pi_flag_c6 = rt->pi_shape_flag;
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Stage 5 — PWM output (FUN_5314 @ 0x5314-0x565f)
+ * ═════════════════════════════════════════════════════════════════════ */
+
+static void s_pwm_output(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal)
+{
+ /* A. eval pwm_A */
+ s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_PWM_A],
+ &rt->pwm_slot_a);
+
+ /* B. eval pwm_B */
+ s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_PWM_B],
+ &rt->pwm_slot_b);
+
+ /* C. combine bilinear */
+ int16_t duty = s_combine(cal->pwm_y_table, &rt->pwm_slot_a, &rt->pwm_slot_b);
+ rt->pwm_duty = (uint16_t)duty;
+
+ /* D. Duty-range classification [0x5361-0x538b] — based on pre-shape duty. */
+ if (rt->pwm_duty < 0x29u) rt->pwm_duty_range_flag = 1u;
+ else if (rt->pwm_duty > 0xFD7u) rt->pwm_duty_range_flag = 2u;
+ else rt->pwm_duty_range_flag = 0u;
+
+ /* E1. RPM-window three-phase matcher [0x53be-0x552a].
+ *
+ * Phase 1 (strict-band): rpm strictly inside any of 4 bands at
+ * cal+0xF2 → slew_increment = +pwm_slew_step.
+ * Phase 2 (hysteresis margin): rpm in any band's halfwidth-extended
+ * region → HOLD (preserve previous slew_increment).
+ * Phase 3 (deep-out): rpm clear of every extended band AND
+ * pwm_period < pwm_period_max → slew_increment = -pwm_slew_step.
+ *
+ * Slew applied as `pwm_period -= slew_increment` (subtraction;
+ * positive increment shrinks period → faster PWM).
+ * pwm_slew_increment lives in rt and persists across cycles so the
+ * HOLD path keeps the previous direction.
+ *
+ * Disasm note: high-edge tests use JC (db) at 0x5463/0x5497 for
+ * bands 0/1 and JNC (d3) at 0x54cb/0x54fd for bands 2/3 — these
+ * encodings produce equivalent control flow (HOLD on rpm < hi+hw,
+ * advance on rpm >= hi+hw). See open-questions.md §5 closeout. */
+ uint16_t *pwm_period = (uint16_t *)&rt->pwm_shape_state[0];
+ const int16_t rpm_s = (int16_t)rt->inputs.rpm;
+ const int16_t halfwidth = cal->pwm_window_halfwidth;
+ const int16_t step_mag = cal->pwm_slew_step;
+
+ /* Phase 1 — strict-band test [0x53be-0x542d] */
+ int slew_set = 0;
+ for (int bi = 0; bi < 4; bi++) {
+ int16_t lo = cal->pwm_rpm_windows[bi * 2];
+ int16_t hi = cal->pwm_rpm_windows[bi * 2 + 1];
+ if (rpm_s > lo && rpm_s < hi) {
+ rt->pwm_slew_increment = step_mag;
+ slew_set = 1;
+ break;
+ }
+ }
+
+ if (!slew_set) {
+ /* Phase 2 — fine hysteresis margin [0x5434-0x54fe] */
+ int phase3 = 1;
+ for (int bi = 0; bi < 4; bi++) {
+ int16_t lo = cal->pwm_rpm_windows[bi * 2];
+ int16_t hi = cal->pwm_rpm_windows[bi * 2 + 1];
+ if (rpm_s <= (int16_t)(lo - halfwidth)) {
+ continue; /* below extended low: try next band */
+ }
+ if (rpm_s < (int16_t)(hi + halfwidth)) {
+ phase3 = 0; /* HOLD */
+ break;
+ }
+ /* rpm_s >= hi + halfwidth: advance to next band */
+ }
+ if (phase3 && *pwm_period < cal->pwm_period_max) {
+ /* Phase 3 — deep-out negative slew [0x54ff-0x551b] */
+ rt->pwm_slew_increment = (int16_t)(-step_mag);
+ }
+ /* else: HOLD (no write) */
+ }
+
+ /* Slew application [0x5520-0x552a] */
+ int32_t period_next = (int32_t)*pwm_period - (int32_t)rt->pwm_slew_increment;
+ if (period_next < 0) period_next = 0;
+ if (period_next > 0xFFFF) period_next = 0xFFFF;
+ *pwm_period = (uint16_t)period_next;
+
+ /* Final clamp [0x552f-0x5575] */
+ if (*pwm_period < cal->pwm_period_min) *pwm_period = cal->pwm_period_min;
+ if (*pwm_period > cal->pwm_period_max) *pwm_period = cal->pwm_period_max;
+
+ /* E2. Shape detail [0x557a-0x5608] */
+ pwm_interp_slot_t shape_slot;
+ s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_SHAPE_EVAL],
+ &shape_slot);
+ int16_t shape_height = s_refine(cal->shape_y_table, &shape_slot);
+ if (shape_height < 0) shape_height = 0;
+ if (shape_height > 0x199) shape_height = 0x199;
+ rt->pwm_shape_state[5] = shape_height;
+
+ /* E3. Shape composition additive [0x55cb-0x560f]. ROM formula:
+ * slope = (period_max - period_min) >> 8
+ * numerator = ((period_max - pwm_period) >> 8) * shape_height
+ * pwm_duty += numerator / slope
+ * Shifts happen before multiply (MCS-96 16-bit intermediate). */
+ int16_t slope = (int16_t)(((int32_t)cal->pwm_period_max
+ - (int32_t)cal->pwm_period_min) >> 8);
+ int16_t pmx_delta = (int16_t)(((int32_t)cal->pwm_period_max
+ - (int32_t)*pwm_period) >> 8);
+ int32_t shape_add = slope ? (MUL_S16(pmx_delta, shape_height)
+ / (int32_t)slope)
+ : 0;
+ int32_t duty_new = (int32_t)rt->pwm_duty + shape_add;
+
+ /* F. Duty bounds clamp [0x5614-0x5636] — ROM reads RAM[0x6058]/[0x605a]
+ * (producers open-questions §3). Lifted to cal->pwm_min/cal->pwm_max
+ * (defaults 205/3890, same as the default family). */
+ if (duty_new < (int32_t)cal->pwm_min) duty_new = (int32_t)cal->pwm_min;
+ if (duty_new > (int32_t)cal->pwm_max) duty_new = (int32_t)cal->pwm_max;
+ rt->pwm_duty = (uint16_t)duty_new;
+
+ /* G. HW shadow writes [0x563b-0x565f] using the slewed pwm_period. */
+ uint32_t on_product = (uint32_t)(*pwm_period) * (uint32_t)rt->pwm_duty;
+ rt->pwm_on_time = (uint16_t)(on_product / 0xFFFu);
+ rt->pwm_off_time = (uint16_t)(*pwm_period - rt->pwm_on_time);
+ rt->pwm_period = *pwm_period;
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Bypass-PI LUT helper: query the ROM Y-table directly with (rpm, fbkw)
+ * as the two axes, applying the same eval+combine+clamp the PWM stage
+ * does. Useful for plotting the static (rpm, fbkw) → duty surface
+ * without the PI controller in the loop.
+ * ═════════════════════════════════════════════════════════════════════ */
+
+uint16_t pwm_lut_duty(const pwm_calibration_t *cal,
+ uint16_t rpm, int16_t fbkw)
+{
+ pwm_submap_descr_t a = pwm_submap_descrs[PWM_SUBMAP_PWM_A];
+ pwm_submap_descr_t b = pwm_submap_descrs[PWM_SUBMAP_PWM_B];
+ int16_t rpm_signed = (int16_t)rpm;
+ a.input_ptr = &rpm_signed;
+ b.input_ptr = &fbkw;
+ pwm_interp_slot_t sa, sb;
+ s_eval_submap(&a, &sa);
+ s_eval_submap(&b, &sb);
+ int16_t duty = s_combine(cal->pwm_y_table, &sa, &sb);
+ if (duty < 205) duty = 205;
+ if (duty > 3890) duty = 3890;
+ return (uint16_t)duty;
+}
+
+/* ═════════════════════════════════════════════════════════════════════
+ * Public entry — pwm_service
+ * Mirrors FUN_77b3 (0x77b3-0x77d8) — 5-call linear dispatcher.
+ * ═════════════════════════════════════════════════════════════════════ */
+
+void pwm_service(pwm_runtime_t *rt)
+{
+ read_inputs(rt);
+
+ const pwm_calibration_t *cal = rt->bound_cal;
+
+ s_setpoint(rt); /* [0x77b3-0x77c7] target_336 = pwm_A interp(rpm) */
+ s_setpoint_can_decode(rt, cal); /* CAN-decoded target (FUN_64c3 in ROM) */
+ s_supervisor(rt); /* [0x77cc] */
+ s_publish_cl(rt, cal);/* [0x77cf] */
+ s_pi_update(rt, cal); /* [0x77d2] */
+ s_pwm_output(rt, cal);/* [0x77d5] */
+}
diff --git a/Core/Advance_Control/pwm_504012.h b/Core/Advance_Control/pwm_504012.h
new file mode 100644
index 0000000..6981cb8
--- /dev/null
+++ b/Core/Advance_Control/pwm_504012.h
@@ -0,0 +1,387 @@
+/**
+ * @file pwm.h (families/t06211/compact_src)
+ * @brief Compact single-header API for the t06211 PWM controller.
+ *
+ * Mirrors the public shape of the default family's `compact_src/pwm.h`
+ * but tailored to t06211's 5-stage pipeline (no standalone max_cl_error
+ * lookup, single-submap setpoint, factor+offset target compute).
+ *
+ * Public API is just two functions:
+ * pwm_init() — one-time setup
+ * pwm_service() — per-cycle update (pulls inputs via getters,
+ * runs the 5-stage pipeline, writes rt outputs)
+ *
+ * External inputs arrive through a getter vtable; each callback returns
+ * one signal in native PWM units.
+ *
+ * Every arithmetic choice mirrors the MCS-96 assembly; see
+ * families/t06211/src/ for per-stage commentary.
+ */
+#ifndef PWM_H
+#define PWM_H
+
+#include
+#include
+#include
+
+/* ── MCS-96 arithmetic primitives ───────────────────────────────────── */
+#define MUL_S16(a, b) ((int32_t)(int16_t)(a) * (int32_t)(int16_t)(b))
+#define MULU_U16(a, b) ((uint32_t)(uint16_t)(a) * (uint32_t)(uint16_t)(b))
+#define CLAMP(v, lo, hi) ((v) < (lo) ? (lo) : (v) > (hi) ? (hi) : (v))
+
+static inline int16_t shra16(int16_t v, unsigned n) {
+ if (!n) return v;
+ uint16_t u = (uint16_t)v, sign = (u >> 15) & 1u;
+ uint16_t m = sign ? (uint16_t)(((uint16_t)~0u) << (16u - n)) : 0u;
+ return (int16_t)((u >> n) | m);
+}
+static inline int32_t shra32(int32_t v, unsigned n) {
+ if (!n) return v;
+ uint32_t u = (uint32_t)v, sign = (u >> 31) & 1u;
+ uint32_t m = sign ? (~(uint32_t)0u << (32u - n)) : 0u;
+ return (int32_t)((u >> n) | m);
+}
+
+/* ══════════════════════════════════════════════════════════════════════
+ * 1D INTERPOLATION SLOT
+ * Layout matches family-1's pwm_interp_slot_t and the family-1 ROM
+ * convention [count*2, range, offset, seg_bytes] — t06211's FUN_6fb8
+ * fingerprints identically, so the same layout applies.
+ * ══════════════════════════════════════════════════════════════════════ */
+typedef struct pwm_interp_slot {
+ int16_t row_stride; /* count * 2 (byte stride for Y-table rows) */
+ int16_t x_interval; /* x[k-1] - x[k] */
+ int16_t x_offset; /* input - x[k] */
+ int16_t y_byte_off; /* k * 2 */
+} pwm_interp_slot_t;
+
+/* ══════════════════════════════════════════════════════════════════════
+ * EXTERNAL INPUTS
+ * Populated each cycle by pwm_service via the getter vtable.
+ * t06211-specific set: no temperature, no angle_dec_cmd in the setpoint
+ * (the family-2 setpoint is single-submap RPM-indexed).
+ * ══════════════════════════════════════════════════════════════════════ */
+typedef struct pwm_inputs {
+ int16_t ckp_in; /* 0x02f8 — sensed position */
+ uint16_t rpm; /* 0x0040 — engine RPM */
+ int16_t angle_dec_cmd; /* 0x0042 — accepted for family-1 API parity; unused here */
+ int16_t inj_qty_demand; /* 0x0044 — CAN: inj quantity demand (PI large-neg gate) */
+ int16_t b_fb_kw; /* CAN: plunger feedback baseline. Gets copied into
+ can_raw_b_fb_kw and decoded by s_setpoint_can_decode
+ (port of FUN_64c3) to produce target. */
+ int16_t state_130; /* 0x0130 — CAN: open/closed-loop discriminant */
+ uint16_t supply_voltage; /* 0x0142 — shape_eval submap input */
+ int16_t temperature; /* 0x0146 — accepted for family-1 API parity; unused here */
+} pwm_inputs_t;
+
+/** Getter vtable. 8-callback layout matches family-1's pwm_input_getters_t
+ * so a single FBKW.c can drive either family's pwm.c without changes.
+ * t06211 does not consume angle_dec_cmd or temperature; those getters are
+ * called and stored into rt->inputs but the pipeline ignores them. */
+typedef struct pwm_input_getters {
+ int16_t (*ckp_in) (void *ctx);
+ uint16_t (*rpm) (void *ctx);
+ int16_t (*angle_dec_cmd) (void *ctx); /* accepted; unused by t06211 */
+ int16_t (*inj_qty_demand)(void *ctx);
+ int16_t (*b_fb_kw) (void *ctx);
+ int16_t (*state_130) (void *ctx);
+ uint16_t (*supply_voltage)(void *ctx);
+ int16_t (*temperature) (void *ctx); /* accepted; unused by t06211 */
+ void *ctx;
+} pwm_input_getters_t;
+
+typedef struct pwm_calibration pwm_calibration_t;
+typedef struct pwm_submap_descr pwm_submap_descr_t;
+
+/* ══════════════════════════════════════════════════════════════════════
+ * RUNTIME STATE
+ * RAM-address comments reference the t06211 MCS-96 map (see
+ * families/t06211/docs/variable-glossary.md).
+ * ══════════════════════════════════════════════════════════════════════ */
+typedef struct pwm_runtime {
+ /* External inputs — refreshed each cycle from getters */
+ pwm_inputs_t inputs;
+
+ /* Async flags.
+ * reset_flag — set on reset edge; supervisor clears.
+ * system_flags_110 — bit 5 forces open-loop; bit 0 is fast-recovery latch.
+ */
+ uint8_t reset_flag; /* 0x002e */
+ uint8_t system_flags_110; /* 0x0110 */
+
+ /* ── Dual-target setpoint architecture ── */
+ /* target (RW5E = RAM[0x005e]): PRIMARY CAN-driven setpoint
+ * computed by FUN_64c3 (called from CAN parser FUN_6192). Drives
+ * the PI controller in both open-loop (FUN_67c4:67e5) and
+ * closed-loop (FUN_67c4:680a) paths, and supervisor error compute
+ * (FUN_7beb:7c20). Formula: raw/2 + setpoint_offset_150 + rw42_state,
+ * clamped >= cal->target_5e_min_clamp. */
+ int16_t target_5e; /* 0x005e — primary CAN-decoded setpoint */
+ /* target_336 (DAT_0336): SECONDARY RPM-derived ceiling computed by
+ * FUN_7168(setpoint_descr) + FUN_77b3:77c7. Used ONLY as upper-clamp
+ * in FUN_672b PI compensation (0x67a9/0x67b0) and FUN_7beb supervisor
+ * (0x7c30/0x7c37). Does not drive control directly. */
+ int16_t target_336; /* 0x0336 — RPM-derived ceiling */
+ /* CAN-staging buffers — written externally before pwm_service so
+ * s_setpoint_can_decode can transform them into target. */
+ int16_t can_raw_b_fb_kw; /* 0x012c — raw b_fb_kw from CAN */
+ int16_t can_aux_12e; /* 0x012e — secondary CAN field (drives rw42_state) */
+ int16_t can_half_12a; /* 0x012a — cached raw>>1 (debug visibility) */
+ /* setpoint_offset_150 (RAM[0x0150]) is copied at runtime_init from
+ * cal.setpoint_offset (mirrors family-1's pattern at
+ * compact_src/pwm.c:66). The REAL ROM derives it at boot via
+ * FUN_6b7b @ 0x6bb5: RAM[0x150] = cal+0x4c - RW1E. Because RW1E's
+ * source at the call site (0x6c46) is in a Ghidra-unrecognised code
+ * region, we expose the field directly through cal so the user can
+ * set it to the match-fitted value without needing to simulate the
+ * full FUN_6b7b boot path. */
+ int16_t setpoint_offset_150; /* 0x0150 */
+ int16_t rw42_state; /* RW42 register — = can_aux_12e when valid */
+ int16_t b_fb_kw_baseline; /* 0x033a — latched snapshot of b_fb_kw on reset */
+ int16_t compensation_angle; /* 0x02b8 — PI-helper output consumed in supervisor */
+ int16_t angle_error_raw; /* 0x033e — supervisor output (raw + ceiling-clamped) */
+ uint16_t cl_enable_counter; /* 0x033c */
+
+ /* ── CL correction (RAM-layout identical to family 1) ── */
+ int16_t cl_correction_raw; /* 0x0176 */
+ int16_t angle_offset; /* 0x017c */
+ int16_t supervisor_state_17e; /* 0x017e — CL accumulator */
+ int16_t pos_error_normalizer; /* 0x0332 */
+ int16_t neg_error_normalizer; /* 0x0334 */
+
+ /* ── Publish + PI ── */
+ int16_t estimated_angle; /* 0x02cc */
+ int16_t angle_error_pi; /* 0x02be */
+ int16_t active_request; /* 0x0046 (RW46) — PI output / PWM feed-forward */
+ uint8_t pi_open_loop_flag; /* 0x02c4 */
+ uint8_t pi_shape_flag; /* 0x02c5 */
+ uint8_t pi_flag_c6; /* 0x02c6 */
+ uint8_t pi_flag_338; /* 0x0338 */
+ /* PI Block-4 error-window bounds (independent int16s, NOT a 32-bit
+ * integrator). Boot-initialised once by FUN_76aa @ 0x76aa — see
+ * docs/open-questions.md §2 (resolved). Trim bytes at 0x0414/0x0416
+ * have no writers in the ROM, so bounds default to ±853 at runtime. */
+ int16_t pi_error_bound_pos; /* 0x0450 — default +853 */
+ int16_t pi_error_bound_neg; /* 0x0452 — default -853 */
+ int16_t pi_state_118; /* 0x0118 */
+ int16_t pi_state_c2; /* 0x02c2 */
+
+ /* PI integrator at {RAM[0x02b4]:RAM[0x02b6]} — was misnamed "b_fb_kw"
+ * across the original glossary. Disasm `disasm_nav rw 0x02b4` shows
+ * only internal writers (FUN_672b, FUN_76aa, FUN_7c85), confirming
+ * this is purely a PI integrator state, not an external/CAN input.
+ * Treated as a signed 32-bit pair: hi at 0x02b4 (the value used in
+ * active_request = comp + pi_b4_state), lo at 0x02b6. */
+ int16_t pi_b4_state; /* 0x02b4 — integrator high word */
+ int16_t pi_b6_state; /* 0x02b6 — integrator low word */
+
+ /* PI compensation scalars — boot-set by FUN_76aa from cal+0x118/0x11A
+ * with optional <<4 byte trims at RAM[0x0410]/[0x0412] (no writers,
+ * default 0). Defaults: 0x0454=+480 (post-scale), 0x0456=+256 (step). */
+ int16_t pi_post_scale_454; /* 0x0454 — multiplier in comp = (err*scale)>>8 */
+ int16_t pi_integ_step_456; /* 0x0456 — multiplier in FUN_7c85 integrator step */
+
+ /* PI integ gain — boot-set by FUN_76aa from cal+0x11C (byte, clamped
+ * ≤15). Used in the "reset" branch as: pi_b4_state = (err*gain)>>4 + ckp.
+ * The runtime field formerly named "pwm_period" was a holdover from
+ * family 1 where 0x0330 had a different semantic; in t06211, 0x0330
+ * is this PI gain. */
+ uint8_t pi_integ_gain_330; /* 0x0330 — boot default 6 (cal+0x11C) */
+
+ /* Anti-windup flag set by FUN_672b clamps; consulted by FUN_7c85
+ * to gate integration direction. Values: 0 (in-range), 1 (clamped
+ * low), 2 (clamped high). */
+ uint8_t pi_flag_c7; /* 0x02c7 */
+
+ /* ── PWM output ── */
+ uint16_t pwm_duty; /* 0x02d2 */
+ uint16_t pwm_on_time; /* 0x02ce */
+ uint16_t pwm_off_time; /* 0x02d0 */
+ /* pwm_period — total period in Timer2 ticks = on_time + off_time.
+ * Mirrored from t06211 RAM[0x02e4] (computed each cycle by the PWM
+ * output stage). Exposed for family-1 FBKW.c API parity
+ * (UpdatePWM(&htim4, ch, pwm_on_time, pwm_period)). */
+ uint16_t pwm_period;
+ uint8_t pwm_duty_range_flag; /* 0x00d1 */
+ pwm_interp_slot_t pwm_slot_a; /* 0x02d4 */
+ pwm_interp_slot_t pwm_slot_b; /* 0x02dc */
+ /* pwm_shape_state[6] mirrors RAM[0x02e4..0x02ee]:
+ * [0] pwm_period working value (RAM 0x02e4)
+ * [1] (unused — was slew_increment, broken out as field below)
+ * [2] (RAM 0x02e8 — ROM band-array ptr, unused in C)
+ * [3] (RAM 0x02ea — ROM halfwidth ptr, unused in C)
+ * [4] (RAM 0x02ec — slew step magnitude, unused; read from cal)
+ * [5] shape_height (RAM 0x02ee, E2 output) */
+ int16_t pwm_shape_state[6]; /* 0x02e4-0x02ee */
+ /* Persistent slew_increment (RAM[0x02e6]). Carried across cycles so
+ * the hysteresis-margin HOLD path can preserve previous direction.
+ * Subtracted from pwm_period each cycle. See open-questions §5. */
+ int16_t pwm_slew_increment; /* 0x02e6 */
+
+ /* ── Bindings (set once by pwm_init) ── */
+ const pwm_calibration_t *bound_cal;
+ const pwm_input_getters_t *bound_getters;
+} pwm_runtime_t;
+
+/* ── Calibration (decoded ROM values) ───────────────────────────────── */
+struct pwm_calibration {
+ /* Scalars (from families/t06211/cal_offsets.py FLASH_OFFSETS) */
+ int16_t large_pos_error_thresh; /* cal+0x10E */
+ int16_t large_neg_error_thresh; /* cal+0x110 */
+ int16_t pi_low_clamp; /* cal+0x120 = -512 — Block-4 low clamp + open-loop lower bound */
+ int16_t pi_high_clamp; /* cal+0x124 = +1707 — Block-4 high clamp */
+
+ /* CAN-decoded setpoint (FUN_64c3) cal constants */
+ int16_t b_fb_kw_upper_bound; /* cal+0x004 = +7680 — raw b_fb_kw upper sanity bound */
+ int16_t b_fb_kw_lower_bound; /* cal+0x006 = -768 — raw b_fb_kw lower sanity bound */
+
+ /* setpoint_offset — static bias added after halving raw b_fb_kw.
+ * Family-1 analog: CAL+0x0052 - CAL+0x0054 (compact_src/pwm.h:210).
+ * For t06211, FUN_6b7b computes the equivalent as cal+0x4c - cal+0x4e
+ * (= 3499 - 4156 = -657) at boot and stores at RAM[0x150].
+ * Copied into runtime.setpoint_offset_150 by runtime_reset. */
+ int16_t setpoint_offset; /* = cal+0x4c - cal+0x4e */
+ int16_t target_5e_min_clamp; /* cal+0x122 = -512 — RW5E lower clamp */
+ int16_t can_aux_12e_max; /* cal+0x002 — upper bound for RAM[0x12e] (FUN_649e) */
+ int16_t error_thresh_114; /* cal+0x114 */
+ int16_t pi_thresh_116; /* cal+0x116 */
+ /* pi_state_118 saturation threshold — when the recovery counter
+ * reaches this, FUN_66a8 latches bit0 of system_flags_110, zeroes
+ * the counter, and reloads pi_state_c2 from error_thresh_114.
+ * Boot-cached at RAM[0x02c0] by FUN_6b7b:0x6bd4. ROM literal 0x0320 = 800. */
+ int16_t pi_sat_count_threshold; /* cal+0x112 */
+ int16_t rpm_threshold_11E; /* cal+0x11E — RPM gate inside FUN_66a8 */
+ /* Closed-loop entry RPM floor (FUN_67c4:0x67d9). Sourced from
+ * RAM[0x605c] (flash mirror; ROM literal 0x01A4 = 420). */
+ int16_t pi_cl_rpm_floor;
+
+ int16_t pwm_detail_x0; /* cal+0x0EE */
+ int16_t pwm_detail_x1; /* cal+0x0F0 */
+ int16_t pwm_cached_ptr_0F2; /* cal+0x0F2 — first RPM-window breakpoint (legacy scalar) */
+ int16_t pwm_cached_ptr_102; /* cal+0x102 — window halfwidth (legacy scalar) */
+ int16_t pwm_const_104; /* cal+0x104 */
+
+ /* RPM-window matching for pwm_period slew (ROM 0x538b-0x5575).
+ * Eight breakpoints at cal+0xF2 define four bands (lo,hi); the
+ * cal+0x102 halfwidth adds hysteresis. RPM INSIDE any band drives
+ * pwm_period toward pwm_period_min (→ non-zero shape contribution
+ * adds ~49 duty ticks). RPM OUTSIDE all bands drives pwm_period
+ * toward pwm_period_max (→ zero contribution; Y-table floor). */
+ int16_t pwm_rpm_windows[8]; /* cal+0xF2 — 4 (lo,hi) pairs */
+ int16_t pwm_window_halfwidth; /* cal+0x102 — hysteresis halfwidth */
+ /* Per-cycle slew step magnitude for pwm_period. Sourced from
+ * cal+0x104 (= 354); cached to RAM[0x02ec] at FUN_5314:0x53b9 and
+ * read at the Phase 1 and Phase 3 sites. Aliases pwm_const_104 —
+ * same offset, semantic name. */
+ int16_t pwm_slew_step; /* cal+0x104 — slew magnitude */
+
+ const int16_t *pwm_y_table; /* cal+0x154 (indirect) */
+ const int16_t *shape_y_table; /* cal+0x15E (indirect) */
+
+ int16_t closed_loop_gain_const; /* cached at ROM 0x6056 = 0x000A */
+
+ uint16_t pwm_period_min; /* hypothesised; see cal_tables_rom.c */
+ uint16_t pwm_period_max; /* hypothesised */
+
+ /* PWM duty clamp bounds — RAM[0x6058]/RAM[0x605a] flash mirrors.
+ * Defaults 0x00CD/0x0F32 match the default family's pwm_min/pwm_max. */
+ uint16_t pwm_min; /* RAM[0x6058] cache */
+ uint16_t pwm_max; /* RAM[0x605a] cache */
+};
+
+/* ── Submap descriptor ──────────────────────────────────────────────── */
+struct pwm_submap_descr {
+ uint16_t flags; /* +0 */
+ const int16_t *input_ptr; /* +2 (bound at runtime) */
+ uint16_t count; /* +4 */
+ const int16_t *x; /* +6 */
+ uint16_t input_addr;
+};
+
+/** Bind descriptor input_ptr fields to rt inputs. */
+static inline void pwm_bind_submap_inputs(
+ pwm_runtime_t *rt,
+ pwm_submap_descr_t *descrs,
+ uint16_t n)
+{
+ for (uint16_t i = 0; i < n; i++) {
+ switch (descrs[i].input_addr) {
+ case 0x0040: descrs[i].input_ptr = (const int16_t *)&rt->inputs.rpm; break;
+ case 0x0044: descrs[i].input_ptr = &rt->inputs.inj_qty_demand; break;
+ case 0x0046: descrs[i].input_ptr = &rt->active_request; break;
+ case 0x0142: descrs[i].input_ptr = (const int16_t *)&rt->inputs.supply_voltage; break;
+ default: descrs[i].input_ptr = NULL; break;
+ }
+ }
+}
+
+/* ══════════════════════════════════════════════════════════════════════
+ * PUBLIC API
+ * ══════════════════════════════════════════════════════════════════════ */
+
+/** pwm_flash_t — placeholder for family-1 API parity. Family-1 has Y-tables
+ * in a separate `pwm_flash_t` struct; t06211 keeps them inside
+ * pwm_calibration_t so this struct is empty. Kept so FBKW.c / callers
+ * can pass &pwm_flash_rom without the call failing. */
+typedef struct pwm_flash { char _unused; } pwm_flash_t;
+
+/** 4-argument init matching family-1's pwm_init signature. The `flash`
+ * pointer is accepted (must be non-NULL — pass &pwm_flash_rom) and
+ * ignored by the t06211 pipeline. */
+void pwm_init(pwm_runtime_t *rt,
+ const pwm_calibration_t *cal,
+ const pwm_flash_t *flash,
+ const pwm_input_getters_t *getters);
+
+void pwm_service(pwm_runtime_t *rt);
+
+/** @brief CKP-edge reset hook — call from the CKP interrupt handler.
+ *
+ * Sets rt->reset_flag = 1 so the next pwm_service() call zeroes the
+ * closed-loop accumulator (supervisor_state_17e) and the enable counter
+ * (cl_enable_counter). Byte store is atomic on all mainstream targets;
+ * no locking required as long as it is not called concurrently with
+ * pwm_service().
+ *
+ * Pattern parity with the default family. See
+ * docs/re-guide-ckp-reset-pattern.md for the full rationale.
+ *
+ * **Variant note for t06211:** the reset_flag producer is **absent from
+ * the ROM** (no STB #1 site writing to 0x002e). The supervisor at 0x7beb
+ * still consumes reset_flag == 1 and clears it, but nothing inside the
+ * ROM ever sets it. Consequence: the accumulator walk-off hazard is
+ * structurally identical to family-1, and the application layer must
+ * drive this hook itself at engine-rev rate (typically 4 pulses per
+ * revolution on VP44, ~12–15 scheduler cycles apart at 1200 rpm with a
+ * 1 kHz scheduler). Without it, supervisor_state_17e integrates forever.
+ */
+static inline void pwm_ckp_isr(pwm_runtime_t *rt) { rt->reset_flag = 1; }
+
+/** Utility: 1D descending-X piecewise-linear lookup. */
+int16_t pwm_interp_lookup(const int16_t *x, const int16_t *y,
+ uint16_t n, int16_t in);
+
+/** Utility: bypass-PI bilinear LUT — eval(pwm_A, rpm), eval(pwm_B, fbkw),
+ * combine over Y-table, then apply [205, 3890] clamp. Use this to query
+ * the ROM Y-table directly as a (rpm, fbkw) lookup, skipping the PI
+ * controller entirely. Returns the clamped duty. */
+uint16_t pwm_lut_duty(const pwm_calibration_t *cal,
+ uint16_t rpm, int16_t fbkw);
+
+/* ── ROM-decoded cal + flash placeholder (defined in cal_tables_rom.c) ── */
+extern const pwm_calibration_t pwm_cal_rom;
+extern const pwm_flash_t pwm_flash_rom;
+
+/** Descriptor array indices. */
+enum pwm_submap_id {
+ PWM_SUBMAP_SETPOINT_INTERP = 0,
+ PWM_SUBMAP_PWM_A = 1,
+ PWM_SUBMAP_PWM_B = 2,
+ PWM_SUBMAP_SHAPE_EVAL = 3,
+ PWM_SUBMAP_COUNT = 4
+};
+
+extern pwm_submap_descr_t pwm_submap_descrs[PWM_SUBMAP_COUNT];
+extern const int16_t *pwm_submap_y_of(uint16_t idx);
+
+#endif /* PWM_H */
diff --git a/Core/CAN_Libs/can_db.c b/Core/CAN_Libs/can_db.c
index 1d40625..ff5e451 100644
--- a/Core/CAN_Libs/can_db.c
+++ b/Core/CAN_Libs/can_db.c
@@ -298,17 +298,17 @@ static CanSymbolDef SYM_ID_SEND1[] = {
#elif defined(T06215)
{ "MESOLL", 0, 12, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &ME, &SCALE_ME, CAN_STORE_FLOAT},
- { "B_PHIAD", 12, 24, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_PHIAD, &SCALE_PHIAD, CAN_STORE_FLOAT},
+ { "B_PHIAD", 12, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_PHIAD, &SCALE_PHIAD, CAN_STORE_FLOAT},
{ "FB_KW", 32, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_FB_KW, &SCALE_FBKW, CAN_STORE_FLOAT},
{ "FB_NW", 48, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_FB_NW, &SCALE_FBKW, CAN_STORE_FLOAT},
#elif defined(T15021) || defined(T31804)
{ "MESOLL", 0, 12, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &ME, &SCALE_ME, CAN_STORE_FLOAT},
- { "B_PHIAD", 12, 24, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_PHIAD, &SCALE_PHIAD, CAN_STORE_FLOAT},
+ { "B_PHIAD", 12, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_PHIAD, &SCALE_PHIAD, CAN_STORE_FLOAT},
{ "FB_KW", 32, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_FB_KW, &SCALE_FBKW, CAN_STORE_FLOAT},
{ "FB_NW", 48, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_FB_NW, &SCALE_FBKW, CAN_STORE_FLOAT},
#elif defined(T06209) || defined(T06211) || defined(T06216)
{ "MESOLL", 0, 12, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &ME, &SCALE_ME, CAN_STORE_FLOAT},
- { "B_PHIAD", 12, 24, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_PHIAD, &SCALE_PHIAD, CAN_STORE_FLOAT},
+ { "B_PHIAD", 12, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_PHIAD, &SCALE_PHIAD, CAN_STORE_FLOAT},
{ "FB_KW", 32, 16, CAN_ENDIAN_INTEL, CAN_SYM_SX, 0,0, &B_FB_KW, &SCALE_FBKW, CAN_STORE_FLOAT},
{ "FB_NW", 48, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_FB_NW, &SCALE_FBKW, CAN_STORE_FLOAT},
#else
diff --git a/Core/Immobilisers/ford_immo.c b/Core/Immobilisers/ford_immo.c
index 6758777..0f5ad0c 100644
--- a/Core/Immobilisers/ford_immo.c
+++ b/Core/Immobilisers/ford_immo.c
@@ -25,8 +25,8 @@ uint16_t FIEONA_FIRSTWORD = 0x2001;
uint16_t FIEONA_SECONDWORD = 0x2000;
static uint16_t _immo_next2 = 0x4801; // predicted next rx = f(0x2001)
-static uint16_t _rx_received = 0;
-static uint8_t _rx_fresh = 0;
+static volatile uint16_t _rx_received = 0;
+static volatile uint8_t _rx_fresh = 0;
// --- Core step function: f(x) = u16(u16(x*x) + (x >> 2)) ---
@@ -39,10 +39,13 @@ static uint16_t immo_f(uint16_t x)
}
// --- Advance: compute next tx and prediction ---
-
+uint8_t notreceived;
uint16_t FIEONA_advance()
{
uint16_t rx = _rx_fresh ? _rx_received : _immo_next2;
+ if(!_rx_fresh){
+ notreceived++;
+ }
_rx_fresh = 0;
// Phase 1: tx = f(rx), fixup if < 2
@@ -88,7 +91,7 @@ extern uint8_t count;
void Fieona_SEND4_Handler(
const struct CanMessageDef *msg,
const uint8_t in_data[8],
- CanTxFn tx
+ CanTxFn txfn
)
{
if(count > 1){
diff --git a/Core/Inc/id.h b/Core/Inc/id.h
index 72c29b2..7797431 100644
--- a/Core/Inc/id.h
+++ b/Core/Inc/id.h
@@ -8,23 +8,20 @@
#ifndef INC_ID_H_
#define INC_ID_H_
+/* 504 012 */
+
/* 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 T15021 //audi 506 -> 030 033
//#define T31804 //audi 506 -> 037 038
//#define T06209 //eq: T06216
-//#define T06211
+#define T06211
-//#define _004006
-//#define _004004
-#define _004002
-//#define _504003
-//#define _504009
-//#define _504010
+#define _504012
/* FORD */
-#define FORD_SYNC_PULSE_OUT 1
+#define FORD_SYNC_PULSE_OUT 0
#define ENABLE_AUDI_IMMO 0
#define HAS_PREINJECTION 0
@@ -32,43 +29,22 @@
#define CYLINDERS 4
/* TIMING COMPENSATIONS */
-#define PHI1 25.4
+#define PHI1 41.016
-#define TEIN_NOMINAL 0
-#define TEIN_FAULT 950.5
+#define TEIN_NOMINAL 1550
+//(phiad - injangle with fault tein)*totime = teinnom
+#define TEIN_FAULT 950
-/* ALL FBKW */
-#define FBKW_DEM_M 0.5
-#define FBKW_DEM_TEMP_M -0.0145
-#define FBKW_DEM_TEMP_N 0.53
+#define FBKW_FEEDBACK_ZERO 53.75
+#define FBKW_FEEDBACK_MIN -5.367
+#define FBKW_FEEDBACK_MAX 21.27
-#define FBKW_DEM_A1 1.42
-#define FBKW_DEM_A2 1.45
-#define FBKW_DEM_A3 -0.459
-
-#define FBKW_DEM_ME_M 0.0031
-#define FBKW_DEM_ME_N 0.527
-
-#define FBKW_DEM_MIN 0
-
-#define FBKW_FEEDBACK_ZERO 7.617
-#define FBKW_FEEDBACK_MIN -3.188
-#define FBKW_FEEDBACK_MAX 17.813
#define FBKW_FEEDBACK_IC_DT 27
-#define FBKW_PID_KP 90 //16
-#define FBKW_PID_KI 0 //18
-#define FBKW_PID_KD 0
-#define FBKW_PID_KAW 0 //16
-#define FBKW_PID_BIAS 60
-#define FBKW_PID_INTEGRAL 0
-#define FBKW_PID_MAXRATE 10000
-#define FBKW_PWM_MAX 95
-#define FBKW_PWM_MIN 5
#define FBKW_MAX 90 //en 504 parece que era 506
-#define FBKW_MAX_REAL_DEM 165
+#define FBKW_MAX_REAL_DEM 165 //wtf is this
/* CAN DEFINITIONS */
#define CAN_BAUDRATE 500
@@ -76,9 +52,9 @@
#define CAN_EMPF2_INSTANT 0
/* ALL FUELMAP */
-#define FM_N_RPM 10
-#define FM_N_ME 12
-#define FM_N_T 6
+#define FM_N_RPM 6
+#define FM_N_ME 11
+#define FM_N_T 5
/* PEAK AND HOLD */
diff --git a/Core/Inc/id_504012.h b/Core/Inc/id_504012.h
index 5d1a7ba..f200d1a 100644
--- a/Core/Inc/id_504012.h
+++ b/Core/Inc/id_504012.h
@@ -18,6 +18,8 @@
//#define T06209 //eq: T06216
#define T06211
+#define _504012
+
/* FORD */
#define FORD_SYNC_PULSE_OUT 0
@@ -45,7 +47,8 @@
#define FBKW_DEM_MIN -6
-#define FBKW_FEEDBACK_ZERO 53.75
+#define FBKW_FEEDBACK_ZERO 53.75//53.75
+
#define FBKW_FEEDBACK_MIN -5.367
#define FBKW_FEEDBACK_MAX 21.27
diff --git a/Core/Kline_Libs/psg_prop.h b/Core/Kline_Libs/psg_prop.h
index f47269e..1c484dd 100644
--- a/Core/Kline_Libs/psg_prop.h
+++ b/Core/Kline_Libs/psg_prop.h
@@ -46,6 +46,12 @@
#define PSG_KUNDENNUMMER1_STR "139031 "
#define PSG_KUNDENNUMMER2_STR " "
#define PSG_MOD_INDEX_STR "000000"
+#elif defined(_504012)
+#define PSG_IDENT_STR "0470504012"
+#define PSG_KUNDENNUMMER_STR "167005M321 "
+#define PSG_KUNDENNUMMER1_STR "167005M321"
+#define PSG_KUNDENNUMMER2_STR " "
+#define PSG_MOD_INDEX_STR "000001"
#else
/* Modification index — 6 ASCII chars, ROM 0x93A7 */
diff --git a/Core/Src/fuel_map.c b/Core/Src/fuel_map.c
index f68a5fa..6894016 100644
--- a/Core/Src/fuel_map.c
+++ b/Core/Src/fuel_map.c
@@ -820,6 +820,64 @@ struct fuelMapIndexes fuelMapI = { //probar este mapa
{ 0.031, 0.406, 1.656, 2.406, 8.313, 24.906, 37.313, 45.688, 60.000, 72.000},
{ -15.0, 5.1 ,30, 50, 74.5}
};
+#elif defined(_504012)
+struct AlphaStruct fuelmap_m12 = { //probar este mapa
+ {
+ { 0.434, 9.152, 9.352, 9.469, 10.430, 10.945, 11.402, 12.246, 13.137, 13.793, 13.887}, //RPM = 99.75
+ { 0.480, 9.668, 10.395, 10.465, 11.027, 12.023, 12.598, 13.852, 14.895, 15.867, 15.973}, //RPM = 199.75
+ { 0.539, 10.969, 11.215, 11.367, 12.504, 14.109, 14.309, 14.625, 15.797, 16.641, 16.652}, //RPM = 399.75
+ { 0.949, 14.871, 15.281, 15.492, 17.109, 18.656, 19.605, 21.832, 23.906, 25.641, 25.641}, //RPM = 999.75
+ { 1.441, 19.441, 19.781, 19.945, 21.434, 23.508, 24.961, 27.926, 31.313, 31.887, 31.898}, //RPM = 1599.75
+ { 1.559, 21.316, 21.855, 21.984, 23.309, 26.672, 28.266, 31.793, 34.453, 34.465, 34.488}, //RPM = 1999.75
+ }
+};
+struct AlphaStruct fuelmap_m5 = { //probar este mapa
+ {
+ { 0.586, 9.305, 9.504, 9.621, 10.582, 11.203, 11.707, 12.668, 13.535, 14.191, 14.273}, //RPM = 99.75
+ { 0.621, 9.809, 10.535, 10.605, 11.168, 12.246, 12.891, 14.191, 15.328, 16.277, 16.383}, //RPM = 199.75
+ { 0.691, 11.121, 11.355, 11.508, 12.645, 14.344, 14.766, 15.480, 16.652, 17.508, 17.496}, //RPM = 399.75
+ { 1.031, 14.953, 15.363, 15.574, 17.191, 18.797, 19.887, 22.254, 24.457, 26.180, 26.191}, //RPM = 999.75
+ { 1.453, 19.453, 19.793, 19.957, 21.445, 23.777, 25.254, 28.500, 32.039, 32.625, 32.625}, //RPM = 1599.75
+ { 1.582, 21.316, 21.879, 22.008, 23.320, 26.953, 28.688, 32.496, 35.309, 35.297, 35.320}, //RPM = 1999.75
+ }
+};
+struct AlphaStruct fuelmap_10 = { //probar este mapa
+ {
+ { 0.738, 9.457, 9.656, 9.773, 10.723, 11.461, 12.000, 13.090, 13.934, 14.590, 14.684}, //RPM = 99.75
+ { 0.762, 9.949, 10.676, 10.746, 11.309, 12.469, 13.195, 14.531, 15.750, 16.711, 16.816}, //RPM = 199.75
+ { 0.844, 11.273, 11.520, 11.672, 12.797, 14.602, 15.246, 16.406, 17.578, 18.434, 18.422}, //RPM = 399.75
+ { 1.113, 15.035, 15.445, 15.656, 17.273, 18.949, 20.191, 22.734, 25.043, 26.777, 26.777}, //RPM = 999.75
+ { 1.477, 19.477, 19.816, 19.980, 21.469, 24.070, 25.594, 29.109, 32.848, 33.422, 33.434}, //RPM = 1599.75
+ { 1.617, 21.387, 21.914, 22.031, 23.367, 27.258, 29.145, 33.293, 36.258, 36.270, 36.281}, //RPM = 1999.75
+ }
+};
+struct AlphaStruct fuelmap_25 = { //probar este mapa
+ {
+ { 0.855, 9.574, 9.773, 9.891, 10.852, 11.684, 12.234, 13.441, 14.262, 14.918, 15.012}, //RPM = 99.75
+ { 0.879, 10.066, 10.793, 10.863, 11.426, 12.645, 13.441, 14.824, 16.102, 17.063, 17.168}, //RPM = 199.75
+ { 0.973, 11.402, 11.648, 11.789, 12.926, 14.813, 15.645, 17.156, 18.328, 19.184, 19.172}, //RPM = 399.75
+ { 1.184, 15.105, 15.516, 15.727, 17.344, 19.078, 20.449, 23.121, 25.535, 27.270, 27.246}, //RPM = 999.25
+ { 1.500, 19.500, 19.840, 20.004, 21.492, 24.316, 25.875, 29.625, 33.516, 34.102, 34.102}, //RPM = 1599.75
+ { 1.629, 21.410, 21.926, 22.043, 23.379, 27.516, 29.520, 33.949, 37.055, 37.066, 37.066}, //RPM = 1999.75
+ }
+};
+struct AlphaStruct fuelmap_60 = { //probar este mapa
+ {
+ { 1.020, 9.738, 9.938, 10.055, 11.016, 11.965, 12.551, 13.887, 14.695, 15.340, 15.434}, //RPM = 99.75
+ { 1.031, 10.219, 10.945, 11.004, 11.566, 12.879, 13.758, 15.188, 16.559, 17.520, 17.613}, //RPM = 199.75
+ { 1.125, 11.555, 11.801, 11.953, 13.090, 15.082, 16.160, 18.117, 19.289, 20.133, 20.145}, //RPM = 399.75
+ { 1.277, 15.199, 15.609, 15.820, 17.438, 19.242, 20.766, 23.613, 26.156, 27.891, 27.891}, //RPM = 999.75
+ { 1.523, 19.523, 19.863, 20.027, 21.516, 24.621, 26.215, 30.281, 34.359, 34.945, 34.945}, //RPM = 1599.75
+ { 1.664, 21.434, 21.949, 22.078, 23.414, 27.832, 30.000, 34.793, 38.051, 38.039, 38.063}, //RPM = 1999.75
+ }
+};
+
+
+struct fuelMapIndexes fuelMapI = { //probar este mapa
+ { 100, 200, 400, 1000, 1600, 2000}, //N_RPM = 17
+ { 0.031, 0.406, 1.656, 2.406, 8.313, 20.813, 29.094, 45.688, 60.000, 70.594, 72.000},
+ { -14.2, 5.25, 29.9, 50, 75.3}
+};
#endif
diff --git a/Core/Src/main.c b/Core/Src/main.c
index fa5f910..70f6bda 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -88,7 +88,6 @@ UART_HandleTypeDef huart1;
FDCAN_FilterTypeDef sFilterConfig;
FDCAN_TxHeaderTypeDef TxHeader;
FDCAN_RxHeaderTypeDef RxHeader;
-PID myPID; //antes era volatile
/* USER CODE END PV */
@@ -1133,7 +1132,7 @@ static void MX_TIM6_Init(void)
htim6.Instance = TIM6;
htim6.Init.Prescaler = 160-1;
htim6.Init.CounterMode = TIM_COUNTERMODE_UP;
- htim6.Init.Period = 999;
+ htim6.Init.Period = 11099;
htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim6) != HAL_OK)
{
diff --git a/Core/Src/toothed_wheel.c b/Core/Src/toothed_wheel.c
index 0687622..9914f3e 100644
--- a/Core/Src/toothed_wheel.c
+++ b/Core/Src/toothed_wheel.c
@@ -271,6 +271,7 @@ void TW_CKP_CAPTURE(){
//meter timeout cuando !count_ckp (significa que solo recibe un pulso);
CKP_PULSE_AVAILABLE = 1;
Timeout_ResetByIndex(13, TIM16->CNT); // Reset CKP timeout
+ FBKW_CKP_ISR();
}
}
float fb_1;
diff --git a/hpsg5-controller_v2-stm32g4.ioc b/hpsg5-controller_v2-stm32g4.ioc
index ade6a39..5cdc0cc 100644
--- a/hpsg5-controller_v2-stm32g4.ioc
+++ b/hpsg5-controller_v2-stm32g4.ioc
@@ -373,7 +373,7 @@ TIM4.PeriodNoDither=29851
TIM4.Prescaler=80-1
TIM4.PulseNoDither_2=5000
TIM6.IPParameters=Prescaler,PeriodNoDither
-TIM6.PeriodNoDither=1000-1
+TIM6.PeriodNoDither=11100-1
TIM6.Prescaler=160-1
TIM7.IPParameters=Prescaler,PeriodNoDither,TIM_MasterOutputTrigger
TIM7.PeriodNoDither=9