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