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

Whole advance control system overhaul. Target angle is now calculated entirely like OEM firmware. Calibration data is fully parsed from a rom dump and a proper file is created with a python extraction tool. Open loop is tested and generates same values as original. In car PID testing has not been done.Voltage compensation is not active as we dont have that parameter's acquisition. 0130 used as flag couldnt be properly decompiled, but logging the ram shows its never blocking the closed loop state. Bench testing is required for verification. Some parameter called setpoint_offset hasnt yet been decompiled, and extractin is through RAM reading. Also modified can_db for T06301 cfgs to include proper FBNW acquisiton

Browse Source
This commit is contained in:
LucianoDev
2026-04-17 23:32:40 +02:00
parent c2b34f085e
commit 0c90da4cfb
13 changed files with 1226 additions and 236 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
.cproject
View File

@@ -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.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.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.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" value="true" valueType="boolean"/> <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"/>
<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"/> <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"/> <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"> <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">
@@ -37,6 +37,7 @@
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/CAN_Libs}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/CAN_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Advance_Control}&quot;"/>
</option> </option>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.1467004031" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/> <inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.1467004031" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/>
</tool> </tool>
@@ -57,6 +58,7 @@
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/CAN_Libs}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/CAN_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Advance_Control}&quot;"/>
</option> </option>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.264989976" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/> <inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.264989976" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
</tool> </tool>
@@ -122,6 +124,7 @@
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/CAN_Libs}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/CAN_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Advance_Control}&quot;"/>
</option> </option>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.1230182628" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/> <inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.1230182628" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/>
</tool> </tool>
@@ -141,6 +144,7 @@
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/CAN_Libs}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/CAN_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Immobilisers}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Kline_Libs}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Core/Advance_Control}&quot;"/>
</option> </option>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.1042657329" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/> <inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.1042657329" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
</tool> </tool>

167
Core/Advance_Control/FBKW.c Normal file
View File

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

14
Core/Inc/FBKW.h → Core/Advance_Control/FBKW.h
View File

@@ -8,6 +8,7 @@
#ifndef INC_FBKW_H_ #ifndef INC_FBKW_H_
#define INC_FBKW_H_ #define INC_FBKW_H_
#include "main.h" #include "main.h"
#include "pwm.h"
extern TIM_HandleTypeDef htim4; extern TIM_HandleTypeDef htim4;
@@ -31,7 +32,7 @@ extern float FBKW_DC;
extern float FBKW_FEEDBACK; extern float FBKW_FEEDBACK;
extern float B_FB_KW; extern float B_FB_KW;
extern float B_FB_NW; extern int16_t B_FB_NW;
extern uint8_t forceDC; extern uint8_t forceDC;
@@ -69,4 +70,15 @@ float UpdateFBKW_OpenDutyCycle(float RPM);
extern void UpdatePID(struct PID *pid); extern void UpdatePID(struct PID *pid);
float F_clamp(float val, float min, float max); float F_clamp(float val, float min, float max);
/* ---- VP44 reverse-engineered pipeline bridge ---------------------------
* FBKW_init : call once at startup (after RPM/Temp/etc globals exist).
* FBKW_service : call once per control tick; pulls getters, runs pipeline,
* writes FBKW_DC and drives htim4/CH2 via UpdateFBKW_MODULATION.
* FBKW_pipeline_runtime : accessor to inspect pipeline state for diagnostics.
*/
void FBKW_init(void);
void FBKW_service(void);
const pwm_runtime_t *FBKW_pipeline_runtime(void);
#endif /* INC_FBKW_H_ */ #endif /* INC_FBKW_H_ */

151
Core/Advance_Control/cal_tables_rom.c Normal file
View File

@@ -0,0 +1,151 @@
/**
* @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_004002_0x0000-40960.bin
* Calibration base (RWA4): 0x9C28
* Flash anchor: 0x9618
* Generated: 2026-04-17 22:03:12
*
* DO NOT EDIT — regenerate with:
* python tools/extract_calibration.py rom_eeprom_dump_004002_0x0000-40960.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 */
};
/* ── 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.h Normal file
View 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_dump_0000-9FFF.bin
* Calibration base (RWA4): 0x9C28
*
* DO NOT EDIT — regenerate with:
* python tools/extract_calibration.py rom_dump_0000-9FFF.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 */

544
Core/Advance_Control/pwm.c Normal file
View File

@@ -0,0 +1,544 @@
/**
* @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;
}
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));
//comp angle is 02b6
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; //this is 012a
int16_t t = (int16_t)(half + rt->setpoint_offset); // offset is 0150
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,
};
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. */

293
Core/Advance_Control/pwm.h Normal file
View File

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

10
Core/CAN_Libs/can_db.c
View File

@@ -56,7 +56,8 @@ extern uint16_t BitStatus;
extern float PHI_AD; extern float PHI_AD;
extern float RPM; extern float RPM;
extern float Temp; extern float Temp;
extern float ME, MEPI, B_FB_KW, B_FB_NW, dFi, B_PHIAD; extern float ME, MEPI, B_FB_KW, dFi, B_PHIAD;
extern int16_t B_FB_NW;
extern uint8_t cilCount, safetySHUTOFF; extern uint8_t cilCount, safetySHUTOFF;
extern uint8_t inj_mode, request_syncout_activation; extern uint8_t inj_mode, request_syncout_activation;
extern uint8_t memWrite; extern uint8_t memWrite;
@@ -83,6 +84,7 @@ static const CanScaleDef SCALE_FBKW = { "FBKW", 3.0f/256.0f, 0.0f };
static const CanScaleDef SCALE_ME = { "mg/H", 1.0f/16.0f, 0.0f }; static const CanScaleDef SCALE_ME = { "mg/H", 1.0f/16.0f, 0.0f };
static const CanScaleDef SCALE_TEMP = { "degC", 1.0f/4.0f, -273.0f }; static const CanScaleDef SCALE_TEMP = { "degC", 1.0f/4.0f, -273.0f };
static const CanScaleDef SCALE_FBKW = { "FBKW", 1.0f/16.0f, 0.0f }; static const CanScaleDef SCALE_FBKW = { "FBKW", 1.0f/16.0f, 0.0f };
static const CanScaleDef SCALE_FBNW = { "FBKW", 1.0f/8.0f, 0.0f };
#else #else
static const CanScaleDef SCALE_ME = { "mg/H", 1.0f/32.0f, 0.0f }; static const CanScaleDef SCALE_ME = { "mg/H", 1.0f/32.0f, 0.0f };
static const CanScaleDef SCALE_TEMP = { "degC", 1.0f/16.0f, -273.0f }; static const CanScaleDef SCALE_TEMP = { "degC", 1.0f/16.0f, -273.0f };
@@ -289,9 +291,11 @@ const CanMessageDef MSG_ID_EMPF3 =
static CanSymbolDef SYM_ID_SEND1[] = { static CanSymbolDef SYM_ID_SEND1[] = {
#if defined(T06301) #if defined(T06301)
{ "MESOLL", 0, 12, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &ME, &SCALE_ME, CAN_STORE_FLOAT}, { "MESOLL", 0, 12, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &ME, &SCALE_ME, CAN_STORE_FLOAT},
//{ "B_PHIAD", 32, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_PHIAD, &SCALE_PHIAD, CAN_STORE_FLOAT},
{ "FB_KW", 12, 12, CAN_ENDIAN_INTEL, CAN_SYM_SX, 0,0, &B_FB_KW, &SCALE_FBKW, CAN_STORE_FLOAT}, { "FB_KW", 12, 12, CAN_ENDIAN_INTEL, CAN_SYM_SX, 0,0, &B_FB_KW, &SCALE_FBKW, CAN_STORE_FLOAT},
//{ "FB_NW", 48, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_FB_NW, &SCALE_FBKW, CAN_STORE_FLOAT}, { "FB_NW", 32, 12, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_FB_NW, &SCALE_FBNW, CAN_STORE_S16},
//{ "B_PHIAD", 32, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_PHIAD, &SCALE_PHIAD, CAN_STORE_FLOAT},
#elif defined(T06215) #elif defined(T06215)
{ "MESOLL", 0, 12, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &ME, &SCALE_ME, CAN_STORE_FLOAT}, { "MESOLL", 0, 12, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &ME, &SCALE_ME, CAN_STORE_FLOAT},
{ "B_PHIAD", 12, 24, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_PHIAD, &SCALE_PHIAD, CAN_STORE_FLOAT}, { "B_PHIAD", 12, 24, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_PHIAD, &SCALE_PHIAD, CAN_STORE_FLOAT},

2
Core/Inc/id.h
View File

@@ -51,7 +51,7 @@
#define FBKW_DEM_MIN 0 #define FBKW_DEM_MIN 0
#define FBKW_FEEDBACK_ZERO 7.617 #define FBKW_FEEDBACK_ZERO 8.135//cambia de modulo a modulo
#define FBKW_FEEDBACK_MIN -3.188 #define FBKW_FEEDBACK_MIN -3.188
#define FBKW_FEEDBACK_MAX 17.813 #define FBKW_FEEDBACK_MAX 17.813
#define FBKW_FEEDBACK_IC_DT 27 #define FBKW_FEEDBACK_IC_DT 27

224
Core/Src/FBKW.c
View File

@@ -1,224 +0,0 @@
/*
* FBKW.c
*
* Created on: Nov 20, 2024
* Author: herli
*/
#include <id.h>
#include "FBKW.h"
#include <stdint.h>
#include <math.h>
#include "injection.h"
#include "toothed_wheel.h"
#include "pre_injection.h"
#include "temperature.h"
float FBKW_DEMAND = 0.0;
float FBKW_DC = 5;
float FBKW_FEEDBACK = 0.0;
float B_FB_KW = 0.0;
float DEMAND_filtered = 0.0;
uint8_t CKP_PULSE_AVAILABLE = 0;
uint8_t FBKW_PID_OPEN = 1; //estaba en 0 antes, igual asi no se peta el pid si no recibe avance
void FBKW_PIDInterrupt(){
UpdateFBKW_DEMAND(B_FB_KW);
if(!TW_RPM_SENSOR_STATE){
FBKW_DC = 5; //audi like this
}
else if(!CKP_PULSE_AVAILABLE || !timer1started){ //Hay timeout o aun no se hizo la primera inyección -> modo open loop
UpdateFBKW_OpenDutyCycle(RPM);
FBKW_PID_OPEN = 1;
}else{
UpdatePID(&myPID);
FBKW_PID_OPEN = 0;
}
if(forceDC){
FBKW_DC = forceDC; //onstart should be 21,8%.
}
UpdateFBKW_MODULATION(&htim4, TIM_CHANNEL_2, FBKW_DC);//FBKW_DC
}
float F_clamp(float value, float min, float max) {
if (value < min) {
return min;
} else if (value > max) {
return max;
} else {
return value;
}
}
float UpdateFBKW_DEMAND(float FBKW){
if(FBKW < FBKW_MAX && FBKW > -FBKW_MAX){ //cuando envia 760, se tiene que quedar en el ultimo valor
/*FBKW_DEMAND = FBKW > 90 ? 0 : FBKW * FBKW_DEM_M + FBKW_DEM_N; //el limite superior de 90 se respeta
float variabledemand = fclamp(1.82+ 0.0024724*RPM - 0.2626, 0, 10);
FBKW_DEMAND += variabledemand;*/
float FBKW_DEM_0 = FBKW * FBKW_DEM_M;
float FBKW_TEMP = FBKW_DEM_TEMP_N + FBKW_DEM_TEMP_M * Temp;
float rpm_cor = RPM > 1 ? RPM / 1000 : 0;
float FBKW_DEM_RPM = FBKW_DEM_A1*rpm_cor + FBKW_DEM_A2*rpm_cor*rpm_cor + FBKW_DEM_A3*rpm_cor*rpm_cor*rpm_cor;
FBKW_DEMAND = FBKW_DEM_0 + FBKW_TEMP + FBKW_DEM_RPM;
FBKW_DEMAND = FBKW_DEMAND < FBKW_DEM_MIN ? FBKW_DEM_MIN : FBKW_DEMAND;
DEMAND_filtered = FBKW_DEMAND;
forceDC = 0;
if(FBKW_DEMAND < FBKW_FEEDBACK + 1.66){
Timeout_ResetByIndex(8, TIM16->CNT); // Reset CKP timeout
}
return DEMAND_filtered;
}
else{
forceDC = 0;
return FBKW_DEMAND;
}
}
float UpdateFBKW_OpenDutyCycle(float RPM){
if(RPM > 0){
if(RPM > 200 && DEMAND_filtered <= 0){
FBKW_DC = FBKW_PWM_MAX;
}else{
float a = 3.33*DEMAND_filtered-97.4;
float b = -0.894*DEMAND_filtered+24.6;
FBKW_DC = a+b*log(RPM);
FBKW_DC = F_clamp(FBKW_DC, FBKW_PWM_MIN, FBKW_PWM_MAX);
}
}
return FBKW_DC;
}
void UpdateFBKW_MODULATION(TIM_HandleTypeDef* pwmHandle, uint32_t pwmChannel, float dutyCycle){
//dutyCycle = F_clamp(dutyCycle, 5.0, 95.0);
uint32_t newRegVal = (uint32_t)roundf((float)(pwmHandle->Instance->ARR) * (dutyCycle * 0.01f));
/*In case of the dutyCycle being calculated as higher than the reload register, cap it to the reload register*/
if(newRegVal > pwmHandle->Instance->ARR)
{
newRegVal = pwmHandle->Instance->ARR;
}
/*Assign the new dutyCycle count to the capture compare register.*/
__HAL_TIM_SET_COMPARE(pwmHandle, pwmChannel, (uint32_t)(roundf(newRegVal)));
}
void definePID(struct PID *pid, float Kp, float Ki, float Kd, float Kaw, float Bias, float T_C, float T, float max, float min, float max_rate, float integral, float err_prev, float deriv_prev, float command_sat_prev, float command_prev){
myPID.Kp = Kp;
myPID.Ki = Ki;
myPID.Kd = Kd;
myPID.Kaw = Kaw;
myPID.Bias = Bias;
myPID.T_C = T_C; // Time constant for derivative filtering
myPID.T = T; // Time step
myPID.max = max;
myPID.min = min;
myPID.max_rate = max_rate;
myPID.integral = integral;
myPID.err_prev = err_prev;
myPID.deriv_prev = deriv_prev;
myPID.command_sat_prev = command_sat_prev;
myPID.command_prev = command_prev;
}
void initPID(struct PID *pid, float T_C, float T){
myPID.Kp = FBKW_PID_KP;
myPID.Ki = FBKW_PID_KI;
myPID.Kd = FBKW_PID_KD;
myPID.Kaw = FBKW_PID_KAW;
myPID.Bias = FBKW_PID_BIAS;
myPID.T_C = T_C; // Time constant for derivative filtering
myPID.T = T; // Time step
myPID.max = FBKW_PWM_MAX;
myPID.min = FBKW_PWM_MIN;
myPID.max_rate = FBKW_PID_MAXRATE;
myPID.integral = FBKW_PID_INTEGRAL;
myPID.err_prev = 0;
myPID.deriv_prev = 0;
myPID.command_sat_prev = 0;
myPID.command_prev = 0;
}
void UpdatePID(struct PID *pid)
{
/* This function implements a PID controller.
*
* Inputs:
* measurement: current measurement of the process variable
* setpoint: desired value of the process variable
* pid: a pointer to a PID struct containing the controller parameters
*
* Returns:
* command_sat: the control output of the PID controller (saturated based on max. min, max_rate)
*/
float err;
float command;
float command_sat;
float deriv_filt;
/* Error calculation */
//err = FBKW_FEEDBACK - DEMAND_filtered;
err = FBKW_FEEDBACK - FBKW_DEMAND;
//err = FBKW_DEMAND - FBKW_FEEDBACK;
/* Integral term calculation - including anti-windup */
pid->integral += pid->Ki*err*pid->T + pid->Kaw*(pid->command_sat_prev - pid->command_prev)*pid->T;
/* Derivative term calculation using filtered derivative method */
deriv_filt = (err - pid->err_prev + pid->T_C*pid->deriv_prev)/(pid->T + pid->T_C);
pid->err_prev = err;
pid->deriv_prev = deriv_filt;
/* Summing the 3 terms */
command = pid->Kp*err + pid->integral + pid->Kd*deriv_filt + pid->Bias;
/* Remember command at previous step */
pid->command_prev = command;
/* Saturate command */
if (command > pid->max)
{
command_sat = pid->max;
}
else if (command < pid->min)
{
command_sat = pid->min;
}
else
{
command_sat = command;
}
/* Apply rate limiter */
if (command_sat > pid->command_sat_prev + pid->max_rate*pid->T)
{
command_sat = pid->command_sat_prev + pid->max_rate*pid->T;
}
else if (command_sat < pid->command_sat_prev - pid->max_rate*pid->T)
{
command_sat = pid->command_sat_prev - pid->max_rate*pid->T;
}
else
{
/* No action */
}
/* Remember saturated command at previous step */
pid->command_sat_prev = command_sat;
FBKW_DC = command_sat;
//return command_sat;
}

6
Core/Src/main.c
View File

@@ -180,7 +180,7 @@ void StartSampling(void){
} }
float B_PHIAD = 0; float B_PHIAD = 0;
float B_FB_NW = 0; int16_t B_FB_NW = 0;
float B_KW_N = 0; float B_KW_N = 0;
float PID_PERIOD = 10.0; //in ms float PID_PERIOD = 10.0; //in ms
@@ -339,9 +339,9 @@ int main(void)
//CAN_AppInit(); //CAN_AppInit();
init_FuelMap(&PHI_AD); init_FuelMap(&PHI_AD);
FBKW_init();
//definePID(&myPID, 90, 0, 0, 0.0, 60.0, 40.0*PID_PERIOD/1000, 1.0*PID_PERIOD/1000, 95, 5, 10000, 0.0, 0, 0, 0, 0); //va //definePID(&myPID, 90, 0, 0, 0.0, 60.0, 40.0*PID_PERIOD/1000, 1.0*PID_PERIOD/1000, 95, 5, 10000, 0.0, 0, 0, 0, 0); //va
initPID(&myPID, 40.0*PID_PERIOD/1000, 1.0*PID_PERIOD/1000); //va //initPID(&myPID, 40.0*PID_PERIOD/1000, 1.0*PID_PERIOD/1000); //va
TIM1->DIER |= TIM_DIER_CC1IE | TIM_DIER_CC2IE | TIM_DIER_CC4IE; // | TIM_DIER_CC3IE TIM1->DIER |= TIM_DIER_CC1IE | TIM_DIER_CC2IE | TIM_DIER_CC4IE; // | TIM_DIER_CC3IE
// During TIM1 init (once): // During TIM1 init (once):

4
Core/Src/stm32g4xx_it.c
View File

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

2
Core/Src/timeouts.c
View File

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