diff --git a/.cproject b/.cproject index b7ce1d0..b2a5138 100644 --- a/.cproject +++ b/.cproject @@ -25,7 +25,7 @@ @@ -57,6 +58,7 @@ + @@ -122,6 +124,7 @@ + @@ -141,6 +144,7 @@ + diff --git a/Core/Advance_Control/FBKW.c b/Core/Advance_Control/FBKW.c new file mode 100644 index 0000000..27713a6 --- /dev/null +++ b/Core/Advance_Control/FBKW.c @@ -0,0 +1,167 @@ +/* + * FBKW.c + * + * Created on: Nov 20, 2024 + * Author: herli + */ +#include +#include "FBKW.h" +#include +#include +#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; +} diff --git a/Core/Inc/FBKW.h b/Core/Advance_Control/FBKW.h similarity index 80% rename from Core/Inc/FBKW.h rename to Core/Advance_Control/FBKW.h index 25a153c..12cb08a 100644 --- a/Core/Inc/FBKW.h +++ b/Core/Advance_Control/FBKW.h @@ -8,6 +8,7 @@ #ifndef INC_FBKW_H_ #define INC_FBKW_H_ #include "main.h" +#include "pwm.h" extern TIM_HandleTypeDef htim4; @@ -31,7 +32,7 @@ extern float FBKW_DC; extern float FBKW_FEEDBACK; extern float B_FB_KW; -extern float B_FB_NW; +extern int16_t B_FB_NW; extern uint8_t forceDC; @@ -69,4 +70,15 @@ float UpdateFBKW_OpenDutyCycle(float RPM); extern void UpdatePID(struct PID *pid); 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_ */ diff --git a/Core/Advance_Control/cal_tables_rom.c b/Core/Advance_Control/cal_tables_rom.c new file mode 100644 index 0000000..e401f67 --- /dev/null +++ b/Core/Advance_Control/cal_tables_rom.c @@ -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 */ +}; diff --git a/Core/Advance_Control/cal_tables_rom.h b/Core/Advance_Control/cal_tables_rom.h new file mode 100644 index 0000000..340798a --- /dev/null +++ b/Core/Advance_Control/cal_tables_rom.h @@ -0,0 +1,39 @@ +/** + * @file cal_tables_rom.h + * @brief Extern declarations for ROM-extracted calibration data (compact). + * + * AUTO-GENERATED by tools/extract_calibration.py + * Source ROM: rom_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 */ diff --git a/Core/Advance_Control/pwm.c b/Core/Advance_Control/pwm.c new file mode 100644 index 0000000..a3cba5a --- /dev/null +++ b/Core/Advance_Control/pwm.c @@ -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. */ diff --git a/Core/Advance_Control/pwm.h b/Core/Advance_Control/pwm.h new file mode 100644 index 0000000..4cd26bb --- /dev/null +++ b/Core/Advance_Control/pwm.h @@ -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 +#include +#include + +/* ── MCS-96 arithmetic primitives ───────────────────────────────────── */ +#define MUL_S16(a, b) ((int32_t)(int16_t)(a) * (int32_t)(int16_t)(b)) +#define MULU_U16(a, b) ((uint32_t)(uint16_t)(a) * (uint32_t)(uint16_t)(b)) +#define CLAMP(v, lo, hi) ((v) < (lo) ? (lo) : (v) > (hi) ? (hi) : (v)) +#define INTEG_HI(s) ((int16_t)(((s) >> 16) & 0xFFFF)) +#define INTEG_LO(s) ((uint16_t)((s) & 0xFFFF)) + +/** Portable signed right shift — guarantees sign extension (SHRA/SHRAL). */ +static inline int16_t shra16(int16_t v, unsigned n) { + if (!n) return v; + uint16_t u = (uint16_t)v, sign = (u >> 15) & 1u; + uint16_t m = sign ? (uint16_t)(((uint16_t)~0u) << (16u - n)) : 0u; + return (int16_t)((u >> n) | m); +} +static inline int32_t shra32(int32_t v, unsigned n) { + if (!n) return v; + uint32_t u = (uint32_t)v, sign = (u >> 31) & 1u; + uint32_t m = sign ? (~(uint32_t)0u << (32u - n)) : 0u; + return (int32_t)((u >> n) | m); +} + +/* ── Mode flag bit definitions (address 0x028c) ─────────────────────── */ +#define MODE_FIRST_CALL 0x01 /* open→closed transition */ +#define MODE_OUTER_REGION 0x02 /* error outside center region */ +#define MODE_NEG_SAT 0x04 /* lower saturation */ +#define MODE_POS_SAT 0x08 /* upper saturation */ +#define MODE_LARGE_POS 0x10 /* large positive error */ +#define MODE_LARGE_NEG 0x20 /* large negative error */ +#define MODE_PREV_MASK 0xC0 /* shadow of prev cycle bits 4-5 */ + +#define INTERP_MAX_ENTRIES 16 + +/* ══════════════════════════════════════════════════════════════════════ + * 1D INTERPOLATION SLOT + * Decoded by eval_submap once per input, then consumed by combine_submaps + * (bilinear, takes two slots) or refine_submap (1D, takes one slot). + * Four int16s; no cycle-to-cycle state. + * ══════════════════════════════════════════════════════════════════════ */ +typedef struct pwm_interp_slot { + int16_t row_stride; /* count * 2 — Y-table row byte-stride (A-axis width) */ + int16_t x_interval; /* x[k-1] - x[k] — denominator (dx between breaks) */ + int16_t x_offset; /* input - x[k] — numerator (distance from lower break) */ + int16_t y_byte_off; /* k * 2 — Y-table byte-offset at break k */ +} pwm_interp_slot_t; + +/* ══════════════════════════════════════════════════════════════════════ + * EXTERNAL INPUTS + * Populated each cycle by pwm_service via the getter vtable. All values + * must be in native PWM units — do any unit/scale conversion inside your + * getter. Address comments are the MCS-96 RAM location of the original + * variable in the ROM (for cross-reference with disassembly only). + * ══════════════════════════════════════════════════════════════════════ */ +typedef struct pwm_inputs { + int16_t ckp_in; /* 0x02f8 — CKP-derived sensed position */ + uint16_t rpm; /* 0x0040 — engine RPM */ + int16_t angle_dec_cmd; /* 0x0042 — CAN: angle-decrease command (sp2_B submap input) */ + int16_t inj_qty_demand; /* 0x0044 — CAN: injection quantity demand (sp0_B + PI large-neg threshold) */ + int16_t b_fb_kw; /* 0x02b4 — CAN: B_FB_KW plunger feedback demand (setpoint baseline) */ + int16_t state_130; /* 0x0130 — CAN: open/closed-loop discriminant (< 0 forces open-loop) */ + uint16_t supply_voltage; /* 0x0142 — battery / supply voltage (shape_eval submap input) */ + int16_t temperature; /* 0x0146 — temperature (sp1_B submap input) */ + /* NOTE: pwm_B's submap input_var=0x0046 in the ROM. That address is RW46, + * the register into which finalize_angle_request writes active_request + * at exit (disasm 0x73f4). It is NOT an external input; our port binds + * that descriptor's input_ptr directly to rt->active_request. */ +} pwm_inputs_t; + +/** Getter vtable. Each callback reads one external signal from your system + * and returns it in native PWM units. `ctx` is opaque to the PWM library + * and is passed back unchanged so your getters can reach their own state. + * All callbacks are required (no NULL entries). + */ +typedef struct pwm_input_getters { + int16_t (*ckp_in) (void *ctx); + uint16_t (*rpm) (void *ctx); + int16_t (*angle_dec_cmd) (void *ctx); + int16_t (*inj_qty_demand)(void *ctx); + int16_t (*b_fb_kw) (void *ctx); + int16_t (*state_130) (void *ctx); + uint16_t (*supply_voltage)(void *ctx); + int16_t (*temperature) (void *ctx); + void *ctx; /* user data; forwarded to every getter */ +} pwm_input_getters_t; + +/* Forward decls for the vtables */ +typedef struct pwm_calibration pwm_calibration_t; +typedef struct pwm_flash pwm_flash_t; +typedef struct pwm_submap_descr pwm_submap_descr_t; + +/* ══════════════════════════════════════════════════════════════════════ + * RUNTIME STATE + * Holds everything that persists cycle-to-cycle plus per-cycle working + * values. External inputs live in rt->inputs (populated each cycle). + * ══════════════════════════════════════════════════════════════════════ */ +typedef struct pwm_runtime { + /* External inputs — refreshed each cycle by pwm_service from getters */ + pwm_inputs_t inputs; + + /* Async / mixed-direction flags. + * reset_flag — set by user/ISR on reset edge; cleared by supervisor. + * system_flags_110 — bit 5 (0x20): external "force open-loop" request; + * bit 0 (0x01): internal latch set by fast_recovery, + * cleared by PI when error_persist_counter hits zero. + */ + uint8_t reset_flag; /* 0x002e */ + uint8_t system_flags_110; /* 0x0110 */ + + /* ── Setpoint pipeline ───────────────────────────────────────────── */ + int16_t compensation_angle; /* 0x02b6 — sum of 3 submap-pair outputs */ + int16_t pre_offset_target; /* 0x012a — (b_fb_kw + compensation_angle) >> 1 */ + int16_t setpoint_offset; /* 0x0150 — user-supplied setpoint bias */ + int16_t target; /* RW5E — final setpoint (clamped) */ + pwm_interp_slot_t setpoint_slot_x; /* 0x02b8 — per-call eval → combine */ + pwm_interp_slot_t setpoint_slot_y; /* 0x02c0 */ + + /* ── Supervisor ──────────────────────────────────────────────────── */ + int16_t cl_enable_counter; /* 0x0340 */ + int16_t cl_error_delta; /* 0x0342 */ + int16_t max_cl_error; /* 0x02f2 */ + int16_t supervisor_state_17e; /* 0x017e — CL-correction accumulator */ + + /* ── PI controller ───────────────────────────────────────────────── */ + int16_t angle_error; /* 0x0278 */ + int16_t angle_error_shaped; /* 0x0276 */ + uint8_t mode_flags; /* 0x028c */ + int32_t integrator_state; /* 0x0288/028a */ + int16_t integrator_gain; /* 0x0286 */ + int16_t active_request; /* 0x0274 — final angle request */ + int16_t estimated_angle; /* 0x02cc */ + int16_t angle_offset; /* 0x017c */ + int16_t cl_correction_raw; /* 0x0176 */ + int16_t recovery_counter; /* 0x0118 */ + int16_t recovery_count_threshold;/* 0x027a */ + int16_t error_persist_counter; /* 0x027c */ + int16_t first_call_gain; /* 0x02ec */ + int16_t pos_error_normalizer; /* 0x02ee */ + int16_t neg_error_normalizer; /* 0x02f0 */ + int16_t min_rpm_openloop; /* 0x015c */ + + /* PI shaping (populated from CAL by pwm_init) */ + int16_t upper_breakpoint, lower_breakpoint; + int16_t center_slope, upper_outer_slope, lower_outer_slope; + int16_t upper_integrator_gain, center_integrator_gain, lower_integrator_gain; + + /* ── PWM output ──────────────────────────────────────────────────── */ + uint16_t pwm_duty; /* 0x02d2 */ + uint16_t pwm_period; /* 0x0330 */ + uint16_t pwm_on_time, pwm_off_time; /* 0x02ce / 0x02d0 — HW compare regs */ + uint8_t pwm_status_flag; /* 0x00d1 */ + int16_t shape_scale; /* 0x0338 */ + int16_t shape_intermediate; /* 0x0332 */ + int16_t shape_output; /* 0x033a */ + uint16_t pwm_min, pwm_max; /* 0x0158 / 0x015a */ + pwm_interp_slot_t pwm_slot_x; /* 0x0290 — pwm_A eval → combine */ + pwm_interp_slot_t pwm_slot_y; /* 0x0298 — pwm_B eval → combine */ + + /* ── Bindings (set once by pwm_init, consumed by pwm_service) ───── */ + const pwm_calibration_t *bound_cal; + const pwm_flash_t *bound_flash; + const pwm_input_getters_t *bound_getters; +} pwm_runtime_t; + +/* ── Calibration (TABLE[RWA4]+offset) ───────────────────────────────── */ +struct pwm_calibration { + int16_t target_minimum; /* CAL+0x11E */ + /* Y-table pointers for each submap pair. Each field originally held a + * 16-bit ROM address (CAL+0x184/186/188). [0]=RPM×InjQty, [1]=RPM×Temp, + * [2]=RPM×AngleDec — see src/submap_inputvars.txt */ + const int16_t *y_pair[3]; /* CAL+0x184/186/188 */ + /* PI shaping (CAL+0x00FE…0x0116) */ + int16_t pi_upper_breakpoint, pi_lower_breakpoint; + int16_t pi_center_slope, pi_upper_outer_slope, pi_lower_outer_slope; + int16_t pi_upper_integrator_gain, pi_center_integrator_gain, pi_lower_integrator_gain; + + /* compute_closed_loop_correction tuning */ + int16_t closed_loop_gain_const; /* CAL+0x0156 */ + + /* fast_recovery tuning */ + int16_t error_persist_init_count; /* CAL+0x0108 */ + int16_t recovery_rpm_threshold; /* CAL+0x011A */ +}; + +/* ── 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 */ diff --git a/Core/CAN_Libs/can_db.c b/Core/CAN_Libs/can_db.c index 675034c..6097653 100644 --- a/Core/CAN_Libs/can_db.c +++ b/Core/CAN_Libs/can_db.c @@ -56,7 +56,8 @@ extern uint16_t BitStatus; extern float PHI_AD; extern float RPM; 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 inj_mode, request_syncout_activation; 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_TEMP = { "degC", 1.0f/4.0f, -273.0f }; static const CanScaleDef SCALE_FBKW = { "FBKW", 1.0f/16.0f, 0.0f }; +static const CanScaleDef SCALE_FBNW = { "FBKW", 1.0f/8.0f, 0.0f }; #else static const CanScaleDef SCALE_ME = { "mg/H", 1.0f/32.0f, 0.0f }; static const CanScaleDef SCALE_TEMP = { "degC", 1.0f/16.0f, -273.0f }; @@ -289,9 +291,11 @@ const CanMessageDef MSG_ID_EMPF3 = static CanSymbolDef SYM_ID_SEND1[] = { #if defined(T06301) { "MESOLL", 0, 12, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &ME, &SCALE_ME, CAN_STORE_FLOAT}, - //{ "B_PHIAD", 32, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_PHIAD, &SCALE_PHIAD, CAN_STORE_FLOAT}, { "FB_KW", 12, 12, CAN_ENDIAN_INTEL, CAN_SYM_SX, 0,0, &B_FB_KW, &SCALE_FBKW, CAN_STORE_FLOAT}, - //{ "FB_NW", 48, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_FB_NW, &SCALE_FBKW, CAN_STORE_FLOAT}, + { "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) { "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}, diff --git a/Core/Inc/id.h b/Core/Inc/id.h index 39c1a29..db3dd2f 100644 --- a/Core/Inc/id.h +++ b/Core/Inc/id.h @@ -51,7 +51,7 @@ #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_MAX 17.813 #define FBKW_FEEDBACK_IC_DT 27 diff --git a/Core/Src/FBKW.c b/Core/Src/FBKW.c deleted file mode 100644 index f834536..0000000 --- a/Core/Src/FBKW.c +++ /dev/null @@ -1,224 +0,0 @@ -/* - * FBKW.c - * - * Created on: Nov 20, 2024 - * Author: herli - */ -#include -#include "FBKW.h" -#include -#include -#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; -} diff --git a/Core/Src/main.c b/Core/Src/main.c index baf9efd..fa28296 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -180,7 +180,7 @@ void StartSampling(void){ } float B_PHIAD = 0; -float B_FB_NW = 0; +int16_t B_FB_NW = 0; float B_KW_N = 0; float PID_PERIOD = 10.0; //in ms @@ -339,9 +339,9 @@ int main(void) //CAN_AppInit(); init_FuelMap(&PHI_AD); - + FBKW_init(); //definePID(&myPID, 90, 0, 0, 0.0, 60.0, 40.0*PID_PERIOD/1000, 1.0*PID_PERIOD/1000, 95, 5, 10000, 0.0, 0, 0, 0, 0); //va - initPID(&myPID, 40.0*PID_PERIOD/1000, 1.0*PID_PERIOD/1000); //va + //initPID(&myPID, 40.0*PID_PERIOD/1000, 1.0*PID_PERIOD/1000); //va TIM1->DIER |= TIM_DIER_CC1IE | TIM_DIER_CC2IE | TIM_DIER_CC4IE; // | TIM_DIER_CC3IE // During TIM1 init (once): diff --git a/Core/Src/stm32g4xx_it.c b/Core/Src/stm32g4xx_it.c index 72632e2..7b17bc9 100644 --- a/Core/Src/stm32g4xx_it.c +++ b/Core/Src/stm32g4xx_it.c @@ -399,8 +399,8 @@ void TIM6_DAC_IRQHandler(void) HAL_TIM_IRQHandler(&htim6); HAL_DAC_IRQHandler(&hdac1); /* USER CODE BEGIN TIM6_DAC_IRQn 1 */ - FBKW_PIDInterrupt(); - + //FBKW_PIDInterrupt(); + FBKW_service(); /* USER CODE END TIM6_DAC_IRQn 1 */ } diff --git a/Core/Src/timeouts.c b/Core/Src/timeouts.c index af6643b..9253b22 100644 --- a/Core/Src/timeouts.c +++ b/Core/Src/timeouts.c @@ -347,7 +347,7 @@ TimeoutEntry timeout_list[] = { { 0, &last_TurnOff_event_tick, 2500, &BitStatus, (1 << 1), 0, Handle_TurnOff_Timeout, 1 }, // 250ms { 0, &last_CKPSignal_event_tick, 5000, &HiddenTimers, (1 << 12), 0, Handle_CKP_NOSIGNAL_Timeout, 0 }, // 10ms - { 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 { 1, &last_PI_tick, 100, &BitStatus, (1 << 14), 0, Handle_PI_Timeout, 1 }, // 500 ms