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

Compare commits

base: Firmware:oem_pwmcontroller
Branches Tags
Firmware:main Firmware:almera Firmware:oem_pwm_phi_controller Firmware:camion Firmware:oem_pwmcontroller
..
compare: Firmware:main
Branches Tags
Firmware:almera Firmware:oem_pwm_phi_controller Firmware:camion Firmware:oem_pwmcontroller Firmware:main

2 Commits
oem_pwmcon ... main

Author SHA1 Message Date
LucianoDev
606090d08c Added 504012 fuelmaps and updated kline data 2026-04-21 11:17:05 +02:00
LucianoDev
40bda00efa cleared dtcs from eeprom and dtc commands, and also fixed 004002 map. 2026-04-20 16:21:49 +02:00
31 changed files with 409 additions and 3890 deletions
Expand all files Collapse all files

8
.cproject
View File

@@ -37,7 +37,6 @@
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/CAN_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Advance_Control}&quot;"/>
</option>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.1467004031" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/>
</tool>
@@ -58,7 +57,6 @@
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/CAN_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Advance_Control}&quot;"/>
</option>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.264989976" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
</tool>
@@ -85,7 +83,7 @@
</toolChain>
</folderInfo>
<sourceEntries>
<entry excluding="Advance_Control/pwm_504012.c|Advance_Control/cal_tables_rom_504012.c|Advance_Control/pwm_004002.c|Advance_Control/cal_tables_rom_004002.c" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
</sourceEntries>
</configuration>
@@ -124,7 +122,6 @@
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/CAN_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Advance_Control}&quot;"/>
</option>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.1230182628" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/>
</tool>
@@ -144,7 +141,6 @@
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/CAN_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Advance_Control}&quot;"/>
</option>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.1042657329" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
</tool>
@@ -171,7 +167,7 @@
</toolChain>
</folderInfo>
<sourceEntries>
<entry excluding="Advance_Control/pwm_504012.c|Advance_Control/cal_tables_rom_504012.c|Advance_Control/pwm_004002.c|Advance_Control/cal_tables_rom_004002.c" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
</sourceEntries>
</configuration>

178
Core/Advance_Control/FBKW.c
View File

@@ -1,178 +0,0 @@
/*
* FBKW.c
*
* Created on: Nov 20, 2024
* Author: herli
*/
#include <id.h>
#include "FBKW.h"
#include <stdint.h>
#include <math.h>
#include "injection.h"
#include "toothed_wheel.h"
#include "pre_injection.h"
#include "temperature.h"
float FBKW_DEMAND = 0.0;
float FBKW_DC = 5;
float FBKW_FEEDBACK = 0.0;
float B_FB_KW = 0.0;
float DEMAND_filtered = 0.0;
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 UpdatePWM(TIM_HandleTypeDef* pwmHandle, uint32_t pwmChannel, uint16_t period_on, uint16_t period){
if(period_on > period)
{
period_on = period;
}
pwmHandle->Instance->ARR = period;
pwmHandle->Instance->CCR2 = period_on;
}
/* =======================================================================
* VP44 reverse-engineered pipeline bridge
* -----------------------------------------------------------------------
* The pipeline in pwm.c consumes 9 integer inputs through a getter vtable.
* Each getter below converts from the project's native float/int variables
* into the integer units the pipeline expects. Getters marked TODO return
* zero until the correct source variable is wired up.
*
* Pipeline output `rt.pwm_duty` is a 12-bit (0..4095) duty command; it is
* converted to a percentage and fed to UpdateFBKW_MODULATION for htim4/CH2.
* =======================================================================
*/
static pwm_runtime_t fbkw_rt;
static pwm_input_getters_t fbkw_getters;
#define CF_KW 85.3333
#define CF_TMS 8.388
#define CF_ME 32
#define CF_T_M 16
#define CF_T_N 4368
#define CF_VOLT 54
/* ckp_in (0x02f8): sensed plunger position — fed by the CKP-derived
* feedback. FBKW_FEEDBACK is already the plunger-position estimate in
* this project. */
static int16_t fbkw_get_ckp_in(void *ctx) {
(void)ctx;
return (int16_t)(FBKW_FEEDBACK*CF_KW);
}
/* rpm (0x0040): engine RPM as uint16. */
static uint16_t fbkw_get_rpm(void *ctx) {
(void)ctx;
float r = RPM;
if (r < 0.0f) r = 0.0f;
if (r > 65535.0f) r = 65535.0f;
return (uint16_t)(r*CF_TMS); //this is the value that uses the original, teeths/ms so 3degree n /ms.
}
/* angle_dec_cmd (0x0042): CAN angle-decrease command.
* TODO: wire to the real source when available (was CAN-delivered on VP44). */
static int16_t fbkw_get_angle_dec_cmd(void *ctx) {
(void)ctx;
return (int16_t)(B_PHIAD*CF_KW);
}
/* inj_qty_demand (0x0044): injection quantity demand.
* TODO: wire to project's injection-qty variable if/when exposed. */
static int16_t fbkw_get_inj_qty_demand(void *ctx) {
(void)ctx;
return (uint16_t)(ME*CF_ME);
}
/* b_fb_kw (0x02b4): KW plunger-feedback demand (baseline setpoint term).
* Same semantic meaning as the project's B_FB_KW global. */
static int16_t fbkw_get_b_fb_kw(void *ctx) {
(void)ctx;
return (int16_t)(B_FB_KW*CF_KW);
}
/* state_130 (0x0130): CAN-delivered open/closed-loop discriminant.
* TODO: wire when the corresponding CAN signal is exposed. Using
* FBKW_PID_OPEN as a provisional mapping (negative = open-loop sentinel). */
static int16_t fbkw_get_state_130(void *ctx) {
(void)ctx;
return 0;
//return (int16_t)(CKP_PULSE_AVAILABLE ? 0 : -1);
}
/* supply_voltage (0x0142): battery/supply voltage.
* TODO: wire to the project's VBAT ADC reading. */
static uint16_t fbkw_get_supply_voltage(void *ctx) {
(void)ctx;
return (uint16_t)(736);//13.5 * its factor 54
}
/* temperature (0x0146): temperature input.
* Temp is already a project-wide temperature variable (temperature.h). */
static int16_t fbkw_get_temperature(void *ctx) {
(void)ctx;
return (int16_t)(Temp*CF_T_M+CF_T_N);
}
void FBKW_init(void) {
fbkw_getters.ckp_in = fbkw_get_ckp_in;
fbkw_getters.rpm = fbkw_get_rpm;
fbkw_getters.angle_dec_cmd = fbkw_get_angle_dec_cmd;
fbkw_getters.inj_qty_demand = fbkw_get_inj_qty_demand;
fbkw_getters.b_fb_kw = fbkw_get_b_fb_kw;
fbkw_getters.state_130 = fbkw_get_state_130;
fbkw_getters.supply_voltage = fbkw_get_supply_voltage;
fbkw_getters.temperature = fbkw_get_temperature;
fbkw_getters.ctx = NULL;
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;
}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);
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_5e * (3.0f / 256.0f);
}
void FBKW_CKP_ISR(void) {
pwm_ckp_isr(&fbkw_rt);
}
const pwm_runtime_t *FBKW_pipeline_runtime(void) {
return &fbkw_rt;
}

49
Core/Advance_Control/FBKW.h
View File

@@ -1,49 +0,0 @@
/*
* FBKW.h
*
* Created on: Nov 20, 2024
* Author: herli
*/
#ifndef INC_FBKW_H_
#define INC_FBKW_H_
#include "main.h"
#include "pwm.h"
extern TIM_HandleTypeDef htim4;
extern uint8_t CKP_PULSE_AVAILABLE;
extern uint8_t FBKW_PID_OPEN;
extern volatile uint32_t IC_SYNCOUT;
extern volatile uint32_t IC_CKP2;
extern volatile float RPM;
extern float refClock;
extern uint8_t safetySHUTOFF;
extern float FBKW_DEMAND;
extern float FBKW_DC;
extern float FBKW_FEEDBACK;
extern float B_FB_KW;
extern int16_t B_FB_NW;
extern uint8_t forceDC;
/*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 FBKW_CKP_ISR();
/* ---- VP44 reverse-engineered pipeline bridge ---------------------------
* FBKW_init : call once at startup (after RPM/Temp/etc globals exist).
* FBKW_service : call once per control tick; pulls getters, runs pipeline,
* writes FBKW_DC and drives htim4/CH2 via UpdateFBKW_MODULATION.
* FBKW_pipeline_runtime : accessor to inspect pipeline state for diagnostics.
*/
void FBKW_init(void);
void FBKW_service(void);
const pwm_runtime_t *FBKW_pipeline_runtime(void);
#endif /* INC_FBKW_H_ */

138
Core/Advance_Control/cal_tables_rom.c
View File

@@ -1,138 +0,0 @@
/**
* @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 };

12
Core/Advance_Control/cal_tables_rom.h
View File

@@ -1,12 +0,0 @@
/**
* @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 */

156
Core/Advance_Control/cal_tables_rom_004002.c
View File

@@ -1,156 +0,0 @@
/**
* @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 */
};

39
Core/Advance_Control/cal_tables_rom_004002.h
View File

@@ -1,39 +0,0 @@
/**
* @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 */

138
Core/Advance_Control/cal_tables_rom_504012.c
View File

@@ -1,138 +0,0 @@
/**
* @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 };

12
Core/Advance_Control/cal_tables_rom_504012.h
View File

@@ -1,12 +0,0 @@
/**
* @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 */

739
Core/Advance_Control/pwm.c
View File

@@ -1,739 +0,0 @@
/**
* @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] */
}

387
Core/Advance_Control/pwm.h
View File

@@ -1,387 +0,0 @@
/**
* @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 <stdint.h>
#include <stdbool.h>
#include <string.h>
/* ── 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 */

543
Core/Advance_Control/pwm_004002.c
View File

@@ -1,543 +0,0 @@
/**
* @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. */

319
Core/Advance_Control/pwm_004002.h
View File

@@ -1,319 +0,0 @@
/**
* @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 <stdint.h>
#include <stdbool.h>
#include <string.h>
/* ── 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 */

739
Core/Advance_Control/pwm_504012.c
View File

@@ -1,739 +0,0 @@
/**
* @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] */
}

387
Core/Advance_Control/pwm_504012.h
View File

@@ -1,387 +0,0 @@
/**
* @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 <stdint.h>
#include <stdbool.h>
#include <string.h>
/* ── 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 */

20
Core/CAN_Libs/can_db.c
View File

@@ -56,8 +56,7 @@ extern uint16_t BitStatus;
extern float PHI_AD;
extern float RPM;
extern float Temp;
extern float ME, MEPI, B_FB_KW, dFi, B_PHIAD;
extern int16_t B_FB_NW;
extern float ME, MEPI, B_FB_KW, B_FB_NW, dFi, B_PHIAD;
extern uint8_t cilCount, safetySHUTOFF;
extern uint8_t inj_mode, request_syncout_activation;
extern uint8_t memWrite;
@@ -84,7 +83,6 @@ static const CanScaleDef SCALE_FBKW = { "FBKW", 3.0f/256.0f, 0.0f };
static const CanScaleDef SCALE_ME = { "mg/H", 1.0f/16.0f, 0.0f };
static const CanScaleDef SCALE_TEMP = { "degC", 1.0f/4.0f, -273.0f };
static const CanScaleDef SCALE_FBKW = { "FBKW", 1.0f/16.0f, 0.0f };
static const CanScaleDef SCALE_FBNW = { "FBKW", 1.0f/8.0f, 0.0f };
#else
static const CanScaleDef SCALE_ME = { "mg/H", 1.0f/32.0f, 0.0f };
static const CanScaleDef SCALE_TEMP = { "degC", 1.0f/16.0f, -273.0f };
@@ -105,7 +103,7 @@ uint8_t QTY_valve_status = 1; //---* tein, --*- feedback, -*-- rpm,
static CanSymbolDef SYM_ID_EMPF1[] = {
#if defined(T06301)
{ "TEIN_STATUS", 8, 1, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &T_ein_status, NULL, CAN_STORE_U8, INVERTED},
{ "QTY_VALVE_STATUS", 9, 1, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &QTY_valve_status, NULL, CAN_STORE_U8, INVERTED},
{ "QTY_VALVE_STATUS", 9, 1, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &QTY_valve_status, NULL, CAN_STORE_U8},
{ "ANGLESENSOR_STATUS", 10, 1, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &hasCapturedTeeth, NULL, CAN_STORE_U8, INVERTED},
{ "CIL_COUNT", 0, 3, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &cilCount, NULL, CAN_STORE_U8},
@@ -194,7 +192,7 @@ static const uint8_t STARTUP_PAYLOAD_2[8] = {0x81, 0x7F, 0x1C, 0x38, 0x1C, 0xFC,
static const uint8_t EPS_0024_PAYLOAD[8] = {0x64, 0x01, 0x01, 0x00, 0x00, 0x00, 0x5B, 0x37}; //0x75 immo 1 0x71 immo 0
static const uint8_t EPS_00B2_PAYLOAD[8] = {0x64, 0x01, 0x01, 0x00, 0x00, 0x21, 0x5B, 0x37};*/
#elif defined(T06211)
static const uint8_t STARTUP_PAYLOAD[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; //0x75 immo 1 0x71 immo 0
static const uint8_t STARTUP_PAYLOAD[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
#elif defined(T06209) || defined(T06216)
static const uint8_t STARTUP_PAYLOAD[8] = {0x00, 0x00, 0x21, 0xD7, 0x46, 0xBA, 0xE9, 0x5F}; //0x75 immo 1 0x71 immo 0
@@ -291,24 +289,22 @@ const CanMessageDef MSG_ID_EMPF3 =
static CanSymbolDef SYM_ID_SEND1[] = {
#if defined(T06301)
{ "MESOLL", 0, 12, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &ME, &SCALE_ME, CAN_STORE_FLOAT},
{ "FB_KW", 12, 12, CAN_ENDIAN_INTEL, CAN_SYM_SX, 0,0, &B_FB_KW, &SCALE_FBKW, CAN_STORE_FLOAT},
{ "FB_NW", 32, 12, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_FB_NW, &SCALE_FBNW, CAN_STORE_S16},
//{ "B_PHIAD", 32, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_PHIAD, &SCALE_PHIAD, CAN_STORE_FLOAT},
{ "FB_KW", 12, 12, 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},
#elif defined(T06215)
{ "MESOLL", 0, 12, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &ME, &SCALE_ME, CAN_STORE_FLOAT},
{ "B_PHIAD", 12, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_PHIAD, &SCALE_PHIAD, CAN_STORE_FLOAT},
{ "B_PHIAD", 12, 24, 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, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_PHIAD, &SCALE_PHIAD, CAN_STORE_FLOAT},
{ "B_PHIAD", 12, 24, 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, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_PHIAD, &SCALE_PHIAD, CAN_STORE_FLOAT},
{ "B_PHIAD", 12, 24, 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

13
Core/Immobilisers/ford_immo.c
View File

@@ -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 volatile uint16_t _rx_received = 0;
static volatile uint8_t _rx_fresh = 0;
static uint16_t _rx_received = 0;
static uint8_t _rx_fresh = 0;
// --- Core step function: f(x) = u16(u16(x*x) + (x >> 2)) ---
@@ -39,13 +39,10 @@ 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
@@ -91,7 +88,7 @@ extern uint8_t count;
void Fieona_SEND4_Handler(
const struct CanMessageDef *msg,
const uint8_t in_data[8],
CanTxFn txfn
CanTxFn tx
)
{
if(count > 1){
@@ -129,7 +126,7 @@ void Fieona_SEND3_Handler(const uint8_t in_data[8]){
FIEONA_TRANS2 = 0x0001;
FIEONA_TRANS2 = 0;
eps_fieona_override = 1;
}else if(!isCar){ //si no es el coche no enviar
}else{
Timeout_StopByIndex(18);
}
Timeout_ResetByIndex(1, TIM16->CNT);

72
Core/Inc/FBKW.h Normal file
View File

@@ -0,0 +1,72 @@
/*
* FBKW.h
*
* Created on: Nov 20, 2024
* Author: herli
*/
#ifndef INC_FBKW_H_
#define INC_FBKW_H_
#include "main.h"
extern TIM_HandleTypeDef htim4;
extern uint8_t CKP_PULSE_AVAILABLE;
extern uint8_t FBKW_PID_OPEN;
extern volatile uint32_t IC_SYNCOUT;
extern volatile uint32_t IC_CKP2;
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;
extern float B_FB_KW;
extern float 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_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);
#endif /* INC_FBKW_H_ */

21
Core/Inc/id.h
View File

@@ -35,13 +35,34 @@
//(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
#define FBKW_DEM_TEMP_N -7.7
//rpm comp
#define FBKW_DEM_A1 0
#define FBKW_DEM_A2 0
#define FBKW_DEM_A3 0
#define FBKW_DEM_MIN -6
#define FBKW_FEEDBACK_ZERO 53.75
#define FBKW_FEEDBACK_MIN -5.367
#define FBKW_FEEDBACK_MAX 21.27
#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 //wtf is this

2
Core/Inc/id_004002.h
View File

@@ -32,7 +32,7 @@
#define CYLINDERS 4
/* TIMING COMPENSATIONS */
#define PHI1 25.4
#define PHI1 24.914
#define TEIN_NOMINAL 0
#define TEIN_FAULT 950.5

3
Core/Inc/id_504012.h
View File

@@ -47,8 +47,7 @@
#define FBKW_DEM_MIN -6
#define FBKW_FEEDBACK_ZERO 53.75//53.75
#define FBKW_FEEDBACK_ZERO 53.75
#define FBKW_FEEDBACK_MIN -5.367
#define FBKW_FEEDBACK_MAX 21.27

9
Core/Kline_Libs/kl_app.c
View File

@@ -48,13 +48,18 @@ static KL_FaultCode s_dtc_list[KL_DTC_TOTAL_RECORDS];
static size_t s_dtc_count = 0;
/* Default test DTCs */
static const KL_FaultCode s_default_dtcs[] = {
/*static const KL_FaultCode s_default_dtcs[] = {
{ 0x5B, 0x20, 0x55 },
{ 0x50, 0x20, 0x03 },
{ 0x56, 0x20, 0x56 },
{ 0x5E, 0x30, 0x60 },
};*/
static const KL_FaultCode s_default_dtcs[] = {
{ 0x00, 0xFF, 0xFF },
{ 0x00, 0xFF, 0xFF },
{ 0x00, 0xFF, 0xFF },
{ 0x00, 0xFF, 0xFF },
};
/* ================================================================
* Password / login bodies (from old kline.c)
* ================================================================ */

14
Core/Kline_Libs/psg_prop.c
View File

@@ -77,19 +77,25 @@ static const RomStrEntry_t s_rom_str_table[] = {
* Stored as a flat array so any-length chunk reads reconstruct correctly.
* Byte values derived from protocol capture.
*/
/*static const uint8_t s_dtc_eeprom_block[64] = {
0x50, 0x20, 0xAB, 0x0A, 0x0E, 0x09, 0x01, 0xED, 0x5A, 0x20, 0xDF, 0x2D, 0x05,
0x00, 0x02, 0x33, 0x5B, 0x20, 0xDF, 0x2D, 0x05, 0x00, 0x03, 0x34, 0x52, 0x20,
0xDF, 0x2D, 0x00, 0x00, 0x04, 0x30, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
}*/
static const uint8_t s_dtc_eeprom_block[64] = {
/* 0x0000-0x000C (13 bytes, DTC MSG 1) */
0x50, 0x20, 0xAB, 0x0A, 0x0E, 0x09, 0x01, 0xED, 0x5A, 0x20, 0xDF, 0x2D, 0x05,
0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
/* 0x000D-0x0019 (13 bytes, DTC MSG 2) */
0x00, 0x02, 0x33, 0x5B, 0x20, 0xDF, 0x2D, 0x05, 0x00, 0x03, 0x34, 0x52, 0x20,
0xFF, 0xFF, 0xFF, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0xFF,
/* 0x001A-0x0026 (13 bytes, DTC MSG 3) */
0xDF, 0x2D, 0x00, 0x00, 0x04, 0x30, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
/* 0x0027-0x0033 (13 bytes, DTC MSG 4) */
0xFF, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0xFF, 0xFF, 0xFF,
/* 0x0034-0x003F (12 bytes, DTC MSG 5) */
0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
};
/*
* DeltaPhi calibration array (0x009A-0x00BD, 36 bytes = 18 x int16 LE).
* 0x8000 entries are "not calibrated" placeholders.

6
Core/Kline_Libs/psg_prop.h
View File

@@ -48,10 +48,10 @@
#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_KUNDENNUMMER_STR " "
#define PSG_KUNDENNUMMER1_STR " "
#define PSG_KUNDENNUMMER2_STR " "
#define PSG_MOD_INDEX_STR "000001"
#define PSG_MOD_INDEX_STR "000000"
#else
/* Modification index — 6 ASCII chars, ROM 0x93A7 */

265
Core/Src/FBKW.c Normal file
View File

@@ -0,0 +1,265 @@
/*
* FBKW.c
*
* Created on: Nov 20, 2024
* Author: herli
*/
#include <id.h>
#include "FBKW.h"
#include <stdint.h>
#include <math.h>
#include "injection.h"
#include "toothed_wheel.h"
#include "pre_injection.h"
#include "temperature.h"
/*
float errorFBKW = 0;
float error_sum = 0;
float prev_errorFBKW = 0;
float targetFBKW = 0;
float Kp = 7; //Kp = 5, and Ki = 0.5
float Ki = 0.0005;
float Kd = 0.005;
float Ku = 1;
float Offset = 50.0;
float intFBKWerror = 0; //add += error * tstep
float Proportional = 0;
double Integrator = 0;
float derivative = 0;
float maxOutput = 45;
*/
float FBKW_DEMAND = 0.0;
float FBKW_DC = 5;
float FBKW_FEEDBACK = 0.0;
float B_FB_KW = 0.0;
float DEMAND_filtered = 0.0;
uint8_t CKP_PULSE_AVAILABLE = 0;
uint8_t FBKW_PID_OPEN = 1; //estaba en 0 antes, igual asi no se peta el pid si no recibe avance
uint8_t forceDC2 = 0;
void FBKW_PIDInterrupt(){
UpdateFBKW_DEMAND(B_FB_KW);
if(!TW_RPM_SENSOR_STATE){
FBKW_DC = 5; //audi like this
}
else if(!CKP_PULSE_AVAILABLE || !timer1started){ //Hay timeout o aun no se hizo la primera inyección -> modo open loop
UpdateFBKW_OpenDutyCycle(RPM);
FBKW_PID_OPEN = 1;
}else{
UpdatePID(&myPID);
FBKW_PID_OPEN = 0;
}
if(forceDC){
FBKW_DC = forceDC; //onstart should be 21,8%.
}
/*if(forceDC2 > 0){
FBKW_DC = forceDC2;
}*/
UpdateFBKW_MODULATION(&htim4, TIM_CHANNEL_2, FBKW_DC);//FBKW_DC
}
float F_clamp(float value, float min, float max) {
if (value < min) {
return min;
} else if (value > max) {
return max;
} else {
return value;
}
}
float FBKW_DEMAND_OFFSET = -2.5;
float UpdateFBKW_DEMAND(float FBKW){
if(FBKW < FBKW_MAX && FBKW > -FBKW_MAX){ //cuando envia 760, se tiene que quedar en el ultimo valor
/*FBKW_DEMAND = FBKW > 90 ? 0 : FBKW * FBKW_DEM_M + FBKW_DEM_N; //el limite superior de 90 se respeta
float variabledemand = fclamp(1.82+ 0.0024724*RPM - 0.2626, 0, 10);
FBKW_DEMAND += variabledemand;*/
float FBKW_DEM_0 = FBKW * FBKW_DEM_M;
float FBKW_TEMP = FBKW_DEM_TEMP_N + FBKW_DEM_TEMP_M * Temp;
float rpm_cor = RPM > 1 ? RPM / 1000 : 0;
float FBKW_DEM_RPM = FBKW_DEM_A1*rpm_cor + FBKW_DEM_A2*rpm_cor*rpm_cor + FBKW_DEM_A3*rpm_cor*rpm_cor*rpm_cor;
FBKW_DEMAND = FBKW_DEM_0 + FBKW_TEMP + FBKW_DEM_RPM;
#if HAS_PREINJECTION
FBKW_DEMAND += FBKW_DEMAND_OFFSET * active_PI; //this is nasty
#endif
FBKW_DEMAND = FBKW_DEMAND < FBKW_DEM_MIN ? FBKW_DEM_MIN : FBKW_DEMAND;
DEMAND_filtered = FBKW_DEMAND;
forceDC = 0;
if(FBKW_DEMAND < FBKW_FEEDBACK + 1.66){
Timeout_ResetByIndex(8, TIM16->CNT); // Reset CKP timeout
}
return DEMAND_filtered;
}/*else if(B_FB_NW == 0){ //esto siempre 0 porque no esta implementado
FBKW_DC = FBKW_PWM_MIN;
forceDC = FBKW_PWM_MIN;
FBKW_DEMAND = 0;
DEMAND_filtered = FBKW_DEMAND;
return 0;
}else{
forceDC = 0;
return FBKW_DEMAND;
}*/
else{
forceDC = 0;
//FBKW_DEMAND = 0;
//DEMAND_filtered = FBKW_DEMAND;
return FBKW_DEMAND;
}
}
float UpdateFBKW_OpenDutyCycle(float RPM){
if(RPM > 0){
if(RPM > 200 && DEMAND_filtered <= 0){
FBKW_DC = FBKW_PWM_MAX;
}else{
float a = 3.33*DEMAND_filtered-97.4;
float b = -0.894*DEMAND_filtered+24.6;
FBKW_DC = a+b*log(RPM);
FBKW_DC = F_clamp(FBKW_DC, FBKW_PWM_MIN, FBKW_PWM_MAX);
}
}else{
//FBKW_DC = 0;
}
return FBKW_DC;
}
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 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){
myPID.Kp = Kp;
myPID.Ki = Ki;
myPID.Kd = Kd;
myPID.Kaw = Kaw;
myPID.Bias = Bias;
myPID.T_C = T_C; // Time constant for derivative filtering
myPID.T = T; // Time step
myPID.max = max;
myPID.min = min;
myPID.max_rate = max_rate;
myPID.integral = integral;
myPID.err_prev = err_prev;
myPID.deriv_prev = deriv_prev;
myPID.command_sat_prev = command_sat_prev;
myPID.command_prev = command_prev;
}
void initPID(struct PID *pid, float T_C, float T){
myPID.Kp = FBKW_PID_KP;
myPID.Ki = FBKW_PID_KI;
myPID.Kd = FBKW_PID_KD;
myPID.Kaw = FBKW_PID_KAW;
myPID.Bias = FBKW_PID_BIAS;
myPID.T_C = T_C; // Time constant for derivative filtering
myPID.T = T; // Time step
myPID.max = FBKW_PWM_MAX;
myPID.min = FBKW_PWM_MIN;
myPID.max_rate = FBKW_PID_MAXRATE;
myPID.integral = FBKW_PID_INTEGRAL;
myPID.err_prev = 0;
myPID.deriv_prev = 0;
myPID.command_sat_prev = 0;
myPID.command_prev = 0;
}
void updatePIDfreq(struct PID *pid, uint8_t millisPID){
TIM6->ARR = millisPID * 1000;
myPID.T = 1.0*millisPID/1000; // Time step
//derivative filtering time constant update not available
}
void UpdatePID(struct PID *pid)
{
/* This function implements a PID controller.
*
* Inputs:
* measurement: current measurement of the process variable
* setpoint: desired value of the process variable
* pid: a pointer to a PID struct containing the controller parameters
*
* Returns:
* command_sat: the control output of the PID controller (saturated based on max. min, max_rate)
*/
float err;
float command;
float command_sat;
float deriv_filt;
/* Error calculation */
//err = FBKW_FEEDBACK - DEMAND_filtered;
err = FBKW_FEEDBACK - FBKW_DEMAND;
//err = FBKW_DEMAND - FBKW_FEEDBACK;
/* Integral term calculation - including anti-windup */
pid->integral += pid->Ki*err*pid->T + pid->Kaw*(pid->command_sat_prev - pid->command_prev)*pid->T;
/* Derivative term calculation using filtered derivative method */
deriv_filt = (err - pid->err_prev + pid->T_C*pid->deriv_prev)/(pid->T + pid->T_C);
pid->err_prev = err;
pid->deriv_prev = deriv_filt;
/* Summing the 3 terms */
command = pid->Kp*err + pid->integral + pid->Kd*deriv_filt + pid->Bias;
/* Remember command at previous step */
pid->command_prev = command;
/* Saturate command */
if (command > pid->max)
{
command_sat = pid->max;
}
else if (command < pid->min)
{
command_sat = pid->min;
}
else
{
command_sat = command;
}
/* Apply rate limiter */
if (command_sat > pid->command_sat_prev + pid->max_rate*pid->T)
{
command_sat = pid->command_sat_prev + pid->max_rate*pid->T;
}
else if (command_sat < pid->command_sat_prev - pid->max_rate*pid->T)
{
command_sat = pid->command_sat_prev - pid->max_rate*pid->T;
}
else
{
/* No action */
}
/* Remember saturated command at previous step */
pid->command_sat_prev = command_sat;
FBKW_DC = command_sat;
//return command_sat;
}

4
Core/Src/fuel_map.c
View File

@@ -534,8 +534,6 @@ struct fuelMapIndexes fuelMapI = { //probar este mapa
#elif defined(_004002)
struct AlphaStruct fuelmap_m12 = { //probar este mapa
{
{ 5.367, 7.371, 7.453, 7.465, 7.512, 8.414, 10.570, 11.320, 11.625, 11.930, 13.383, 14.543}, //RPM = 99
{ 5.133, 7.242, 7.746, 7.898, 8.344, 9.059, 11.191, 11.836, 12.234, 12.621, 13.852, 14.414}, //RPM = 199
{ 5.332, 7.313, 7.395, 7.406, 7.465, 8.379, 10.535, 11.309, 11.602, 11.883, 13.336, 14.496}, //RPM = 99
{ 5.098, 7.219, 7.395, 7.559, 7.992, 9.023, 11.168, 11.754, 11.953, 12.164, 13.395, 14.379}, //RPM = 199
{ 4.125, 6.340, 6.656, 7.008, 7.992, 9.059, 10.898, 11.426, 11.930, 12.434, 13.957, 15.188}, //RPM = 425
@@ -620,7 +618,7 @@ struct AlphaStruct fuelmap_80 = { //probar este mapa
};
struct fuelMapIndexes fuelMapI = { //probar este mapa
{ 100, 200, 425, 500, 950, 1000, 1400, 1500, 2000, 2200}, //N_RPM = 17
{ 100, 200, 425, 500, 900, 1000, 1400, 1500, 2000, 2200}, //N_RPM = 17
{ 0.375, 0.500, 1.625, 2.500, 5.000, 10.000, 30.000, 35.000, 40.000, 45.000, 60.000, 72.000},
{ -15.5, -5.5 ,6.3, 27.5, 47.8, 75}
};

13
Core/Src/main.c
View File

@@ -88,6 +88,7 @@ UART_HandleTypeDef huart1;
FDCAN_FilterTypeDef sFilterConfig;
FDCAN_TxHeaderTypeDef TxHeader;
FDCAN_RxHeaderTypeDef RxHeader;
PID myPID; //antes era volatile
/* USER CODE END PV */
@@ -179,7 +180,7 @@ void StartSampling(void){
}
float B_PHIAD = 0;
int16_t B_FB_NW = 0;
float B_FB_NW = 0;
float B_KW_N = 0;
float PID_PERIOD = 10.0; //in ms
@@ -338,9 +339,9 @@ int main(void)
//CAN_AppInit();
init_FuelMap(&PHI_AD);
FBKW_init();
//definePID(&myPID, 90, 0, 0, 0.0, 60.0, 40.0*PID_PERIOD/1000, 1.0*PID_PERIOD/1000, 95, 5, 10000, 0.0, 0, 0, 0, 0); //va
//initPID(&myPID, 40.0*PID_PERIOD/1000, 1.0*PID_PERIOD/1000); //va
initPID(&myPID, 40.0*PID_PERIOD/1000, 1.0*PID_PERIOD/1000); //va
TIM1->DIER |= TIM_DIER_CC1IE | TIM_DIER_CC2IE | TIM_DIER_CC4IE; // | TIM_DIER_CC3IE
// During TIM1 init (once):
@@ -1073,9 +1074,9 @@ static void MX_TIM4_Init(void)
/* USER CODE END TIM4_Init 1 */
htim4.Instance = TIM4;
htim4.Init.Prescaler = 80-1;
htim4.Init.Prescaler = 160-1;
htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
htim4.Init.Period = 29851;
htim4.Init.Period = 20000;
htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim4) != HAL_OK)
@@ -1132,7 +1133,7 @@ static void MX_TIM6_Init(void)
htim6.Instance = TIM6;
htim6.Init.Prescaler = 160-1;
htim6.Init.CounterMode = TIM_COUNTERMODE_UP;
htim6.Init.Period = 11099;
htim6.Init.Period = 9999;
htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim6) != HAL_OK)
{

4
Core/Src/stm32g4xx_it.c
View File

@@ -399,8 +399,8 @@ void TIM6_DAC_IRQHandler(void)
HAL_TIM_IRQHandler(&htim6);
HAL_DAC_IRQHandler(&hdac1);
/* USER CODE BEGIN TIM6_DAC_IRQn 1 */
//FBKW_PIDInterrupt();
FBKW_service();
FBKW_PIDInterrupt();
/* USER CODE END TIM6_DAC_IRQn 1 */
}

2
Core/Src/timeouts.c
View File

@@ -347,7 +347,7 @@ TimeoutEntry timeout_list[] = {
{ 0, &last_TurnOff_event_tick, 2500, &BitStatus, (1 << 1), 0, Handle_TurnOff_Timeout, 1 }, // 250ms
{ 0, &last_CKPSignal_event_tick, 5000, &HiddenTimers, (1 << 12), 0, Handle_CKP_NOSIGNAL_Timeout, 0 }, // 10ms
{ 1, &last_CKPSensor_event_tick, 3000, &HiddenTimers, (1 << 13), 0, Handle_CKP_SENSOR_Timeout, 0 }, // 10ms, no tendrian que estar started...
{ 0, &last_CKPSensor_event_tick, 3000, &HiddenTimers, (1 << 13), 0, Handle_CKP_SENSOR_Timeout, 0 }, // 10ms, no tendrian que estar started...
#if HAS_PREINJECTION
{ 1, &last_PI_tick, 100, &BitStatus, (1 << 14), 0, Handle_PI_Timeout, 1 }, // 500 ms

1
Core/Src/toothed_wheel.c
View File

@@ -271,7 +271,6 @@ void TW_CKP_CAPTURE(){
//meter timeout cuando !count_ckp (significa que solo recibe un pulso);
CKP_PULSE_AVAILABLE = 1;
Timeout_ResetByIndex(13, TIM16->CNT); // Reset CKP timeout
FBKW_CKP_ISR();
}
}
float fb_1;

6
hpsg5-controller_v2-stm32g4.ioc
View File

@@ -369,11 +369,11 @@ TIM3.Prescaler=160-1
TIM3.TIM_MasterOutputTrigger=TIM_TRGO_UPDATE
TIM4.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2
TIM4.IPParameters=Channel-PWM Generation2 CH2,Prescaler,PeriodNoDither,PulseNoDither_2
TIM4.PeriodNoDither=29851
TIM4.Prescaler=80-1
TIM4.PeriodNoDither=20000
TIM4.Prescaler=160-1
TIM4.PulseNoDither_2=5000
TIM6.IPParameters=Prescaler,PeriodNoDither
TIM6.PeriodNoDither=11100-1
TIM6.PeriodNoDither=10000-1
TIM6.Prescaler=160-1
TIM7.IPParameters=Prescaler,PeriodNoDither,TIM_MasterOutputTrigger
TIM7.PeriodNoDither=9