diff --git a/Core/Advance_Control/FBKW.c b/Core/Advance_Control/FBKW.c index a09bf0d..d04bd35 100644 --- a/Core/Advance_Control/FBKW.c +++ b/Core/Advance_Control/FBKW.c @@ -13,6 +13,7 @@ #include "pre_injection.h" #include "temperature.h" + float FBKW_DEMAND = 0.0; float FBKW_DC = 5; float FBKW_FEEDBACK = 0.0; @@ -173,6 +174,11 @@ void FBKW_CKP_ISR(void) { pwm_ckp_isr(&fbkw_rt); } +float FBKW_GET_CKP_OFFSET(void) { + return (float)get_ckp_zero() * 3.0f / 256.0f; + //return (get_ckp_zero() >> 8) * 3.0f; + +} const pwm_runtime_t *FBKW_pipeline_runtime(void) { return &fbkw_rt; } diff --git a/Core/Advance_Control/FBKW.h b/Core/Advance_Control/FBKW.h index f830dca..d1bbde7 100644 --- a/Core/Advance_Control/FBKW.h +++ b/Core/Advance_Control/FBKW.h @@ -9,6 +9,7 @@ #define INC_FBKW_H_ #include "main.h" #include "pwm.h" +#include "ckp_acquisition.h" extern TIM_HandleTypeDef htim4; @@ -45,5 +46,5 @@ extern void FBKW_CKP_ISR(); void FBKW_init(void); void FBKW_service(void); const pwm_runtime_t *FBKW_pipeline_runtime(void); - +float FBKW_GET_CKP_OFFSET(void); #endif /* INC_FBKW_H_ */ diff --git a/Core/Advance_Control/cal_tables_rom.c b/Core/Advance_Control/cal_tables_rom.c index a01165a..f20ef8e 100644 --- a/Core/Advance_Control/cal_tables_rom.c +++ b/Core/Advance_Control/cal_tables_rom.c @@ -14,9 +14,14 @@ * ~cal+0x9E (the new ADR-SV-* / KW_SIM_UMSCHALTUNG diagnostic fields per * doc.txt 30.08.02). All T06215 cal field roles map to T06235 with the * +0x14 offset; the actual values were re-extracted from this ROM. + * NOTE: there is also a +0x02 shift in the lower region — at minimum + * cal+0x4c → cal+0x4e and cal+0xa0 → cal+0xa2 (both confirmed in FUN_6a4b + * @ 0x6a4b). Lower-region scalars must be re-checked, not assumed unchanged. * * Notable value deltas vs T06215: - * - setpoint_offset = cal+0x4c − cal+0x4e = 4399 − 3499 = +900 (T06215: -648). + * - setpoint_offset = cal+0x4e − cal+0x50 = 3499 − 4228 = -729 (T06215 cal+0x4c−cal+0x4e = -648). + * Cal-block +0x02 shift in this lower region; T06215's cal+0xa0 modulo + * also moves to cal+0xa2 in T06235 (verified in FUN_6a4b @ 0x6a4b). * - init_p_gain_normal = +368 (T06215: +336) * - init_p_slope_large_pos = +2047 (T06215: +1792) * - init_integ_step_normal = +320 (T06215: +256) @@ -112,10 +117,16 @@ const pwm_calibration_t pwm_cal_rom = { /* CAN-decoded setpoint cal constants (lower region — invariant offsets) */ .b_fb_kw_upper_bound = 7680, /* cal+0x004 */ .b_fb_kw_lower_bound = (int16_t)0xFD00, /* cal+0x006 */ - .setpoint_offset = 900, /* cal+0x4c − cal+0x4e = 4399 − 3499 = 900 (T06215: -648) */ + .setpoint_offset = -729, /* cal+0x4e − cal+0x50 = 3499 − 4228 = -729 (T06215 cal+0x4c−cal+0x4e = -648) */ .target_5e_min_clamp = (int16_t)0xFE00, /* cal+0x13E = -512 (T06215 cal+0x12A +0x14) */ .can_aux_12e_max = 1451, /* cal+0x002 */ + /* CKP-zero acquisition (FUN_55e0 + FUN_6a4b chain) — values read from + * rom_eeprom_dump_0000-9FFF_504042.bin at RWA4 + offset. */ + .ckp_zero_anchor = 4228, /* cal+0x050 @ 0x9C08 = 0x1084 */ + .can_dckp_offset_bias = 0, /* cal+0x052 @ 0x9C0A = 0x0000 */ + .ckp_modulus = 7680, /* cal+0x0A2 @ 0x9C5A = 0x1E00 (90° at 256/3°/unit) */ + /* Recovery / sustained-error machinery */ .pi_state_c2_reload = 100, /* cal+0x128 (was T06215 cal+0x114) */ .inj_qty_thresh = 96, /* cal+0x12A (was T06215 cal+0x116) */ diff --git a/Core/Advance_Control/ckp_acquisition.c b/Core/Advance_Control/ckp_acquisition.c new file mode 100644 index 0000000..8fa78e6 --- /dev/null +++ b/Core/Advance_Control/ckp_acquisition.c @@ -0,0 +1,93 @@ +/* + * ckp_acquisition.c + * + * Created on: May 5, 2026 + * Author: herli + * + * Mirrors ROM FUN_55e0 @ 0x55e0 → FUN_6a4b @ 0x6a4b for the CKP-zero + * acquisition path. See ckp_acquisition.h for the data-flow summary. + */ + +#include "ckp_acquisition.h" +#include "pwm.h" /* pwm_cal_rom + shra16 */ +#include "ee_manager.h" + +/* Inputs */ +int16_t B_CKP_OFFSET = 0; +uint8_t commitCKP_offset = 0; + +/* RAM trims (DAT_0430 word, DAT_0404 signed byte) — see header. */ +int16_t CKP_RAM_TRIM_0430 = 0; +int8_t CKP_RAM_TRIM_0404 = 0; + +/* Outputs */ +int16_t dCKP_OFFSET = 0; +int16_t CKP_ZERO_OFFSET = 0; + +/* FUN_6a4b @ 0x6a4b core — DAT_0152 derivation only. + * + * ROM body (fulldissasembly.txt:20025–20061): + * RW1E = cal[+0x50] + DAT_0430 + * RW1C = RW1E - DAT_0434 - cal[+0x52] + (int8_t)DAT_0404 + * while RW1C < 0 : RW1C += cal[+0xa2] ; JGE LAB_6a7e (signed) + * while RW1C > cal[+0xa2] : RW1C -= cal[+0xa2] ; JLE LAB_6a8c (signed) + * DAT_0152 = RW1C + * + * Side-effects (FUN_6c70 R8F/R90 derivation, DAT_02cb/R2C/R2D clears, + * DAT_017a=5, DAT_0278 reload) belong to the CKP-decode synchronization + * stage and are intentionally not ported here — see the plan's + * "Out of scope" section. + */ +static void ckp_recompute_zero_offset(void) +{ + int16_t anchor = (int16_t)((int16_t)pwm_cal_rom.ckp_zero_anchor + + CKP_RAM_TRIM_0430); + + /* LDBSE at fulldissasembly.txt:20038 sign-extends the byte to a + * word before the ADD. */ + int16_t v = (int16_t)(anchor + - dCKP_OFFSET + - pwm_cal_rom.can_dckp_offset_bias + + (int16_t)s_dfi_code); + + int16_t mod = pwm_cal_rom.ckp_modulus; + + /* Verbatim ROM modulo. JGE/JLE in the ROM are signed compares, so + * the loops bracket v into [0, mod] inclusive. Replacing with `%` + * would diverge for negative v under C90/C99 implementation-defined + * sign-of-remainder rules. */ + while (v < 0) v = (int16_t)(v + mod); + while (v > mod) v = (int16_t)(v - mod); + + CKP_ZERO_OFFSET = v; +} + +/* FUN_55e0 @ 0x55e0 entry — detect change in (B_CKP_OFFSET >> 1) and + * update dCKP_OFFSET; on change, FUN_6a4b is invoked. We always re-run + * FUN_6a4b on the first call so consumers see a coherent + * CKP_ZERO_OFFSET even before B_CKP_OFFSET first moves. + */ +int16_t get_ckp_zero(void) +{ + static uint8_t initialized = 0; + + int16_t halved = shra16(B_CKP_OFFSET, 1); + int16_t biased_now = (int16_t)(dCKP_OFFSET + + pwm_cal_rom.can_dckp_offset_bias); + + if (halved != biased_now) { + dCKP_OFFSET = (int16_t)(halved + - pwm_cal_rom.can_dckp_offset_bias); + /* commitCKP_offset stays unused per user direction; future hook + * for the FUN_56d8 flash-write sequence and the DAT_01e7 bit-7 + * persist-pending flag. */ + (void)commitCKP_offset; + ckp_recompute_zero_offset(); + initialized = 1; + } else if (!initialized) { + ckp_recompute_zero_offset(); + initialized = 1; + } + + return CKP_ZERO_OFFSET; +} diff --git a/Core/Advance_Control/ckp_acquisition.h b/Core/Advance_Control/ckp_acquisition.h new file mode 100644 index 0000000..94ec582 --- /dev/null +++ b/Core/Advance_Control/ckp_acquisition.h @@ -0,0 +1,41 @@ +/* + * ckp_acquisition.h + * + * Created on: May 5, 2026 + * Author: herli + * + * Translates the CAN-supplied raw CKP offset (B_CKP_OFFSET, RW134 / + * DAT_0134) into the ROM-equivalent runtime delta dCKP_OFFSET (DAT_0434) + * and then into the canonical CKP zero-crossing offset CKP_ZERO_OFFSET + * (DAT_0152). Mirrors ROM FUN_55e0 @ 0x55e0 → FUN_6a4b @ 0x6a4b. + */ + +#ifndef ADVANCE_CONTROL_CKP_ACQUISITION_H_ +#define ADVANCE_CONTROL_CKP_ACQUISITION_H_ + +#include + +/* Inputs */ +extern int16_t B_CKP_OFFSET; /* RW134 / DAT_0134 — CAN raw input */ +extern uint8_t commitCKP_offset; /* unused for now (commit gate hook) */ + +/* Internal RAM trims (DAT_0430 word, DAT_0404 signed byte). No runtime + * writer was found in this ROM/car (project memory: + * project_t06235_ckp_processing.md), so they sit at 0 unless a future + * step wires them up. Exposed for diagnostic poking. */ +extern int16_t CKP_RAM_TRIM_0430; +extern int8_t CKP_RAM_TRIM_0404; + +/* Outputs */ +extern int16_t dCKP_OFFSET; /* DAT_0434 — output of FUN_55e0 */ +extern int16_t CKP_ZERO_OFFSET; /* DAT_0152 — output of FUN_6a4b */ + +/* Recompute dCKP_OFFSET from B_CKP_OFFSET (FUN_55e0), then recompute + * CKP_ZERO_OFFSET (FUN_6a4b core derivation), then return + * CKP_ZERO_OFFSET. The commit/flash-persist side-effects (FUN_56d8 + * sequence + DAT_01e7 bit-7) are stubbed via commitCKP_offset for a + * future wiring step. + */ +int16_t get_ckp_zero(void); + +#endif /* ADVANCE_CONTROL_CKP_ACQUISITION_H_ */ diff --git a/Core/Advance_Control/pwm.h b/Core/Advance_Control/pwm.h index 420581d..4cd527a 100644 --- a/Core/Advance_Control/pwm.h +++ b/Core/Advance_Control/pwm.h @@ -193,6 +193,11 @@ struct pwm_calibration { int16_t target_5e_min_clamp; /* cal+0x12A */ int16_t can_aux_12e_max; /* cal+0x002 */ + /* CKP-zero acquisition (FUN_55e0 + FUN_6a4b chain) cal constants */ + int16_t ckp_zero_anchor; /* cal+0x050 — FUN_6a4b additive anchor */ + int16_t can_dckp_offset_bias; /* cal+0x052 — FUN_55e0/FUN_6a4b bias */ + int16_t ckp_modulus; /* cal+0x0A2 — FUN_6a4b modulo wrap */ + /* Recovery / sustained-error machinery */ int16_t pi_state_c2_reload; /* cal+0x114 — reload value for pi_state_c2 on latch */ int16_t inj_qty_thresh; /* cal+0x116 — inj-qty threshold for recovery vs reset */ diff --git a/Core/CAN_Libs/can_db.c b/Core/CAN_Libs/can_db.c index ac42409..973c84a 100644 --- a/Core/CAN_Libs/can_db.c +++ b/Core/CAN_Libs/can_db.c @@ -58,8 +58,10 @@ extern float RPM; extern float Temp; extern float ME, MEPI, B_FB_KW, dFi, B_PHIAD; extern int16_t B_FB_NW; +extern int16_t B_CKP_OFFSET; + extern uint8_t cilCount, safetySHUTOFF; -extern uint8_t inj_mode, request_syncout_activation; +extern uint8_t inj_mode, request_syncout_activation, commitCKP_offset; extern uint8_t memWrite; extern uint32_t quart_hour_counter; @@ -95,6 +97,7 @@ static const CanScaleDef SCALE_ME_RAM = { "mg/H", 1.0f/32.0f, 0.0f }; static const CanScaleDef SCALE_DC = { "%", 100.0f/4095.0f, 0.0f }; static const CanScaleDef SCALE_TEIN = { "uS", 1.0f/2.0f, 0.0f }; static const CanScaleDef SCALE_RPM = { "1/min", 1.0f/4.0f, 0.0f }; + static const CanScaleDef SCALE_VOLTAGE= { "V", 0.0185f, -0.05f }; @@ -322,6 +325,14 @@ static CanSymbolDef SYM_ID_SEND4[] = { #elif defined(cfgejemplo) #endif + +static CanSymbolDef SYM_ID_SEND3[] = { +#if defined(T06235) + + { "CKP_OFFSET", 0, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_CKP_OFFSET, &SCALE_FBKW, CAN_STORE_S16}, + { "commit", 16, 1, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &commitCKP_offset, NULL, CAN_STORE_U8}, +#endif +}; /* ===== RX: ID 0x720 SEND ===== */ static CanSymbolDef SYM_ID_SEND_CUSTOM[] = { { "PHI_OFFSET", 0, 16, CAN_ENDIAN_INTEL, CAN_SYM_SX, 0,0, &dFi, &SCALE_DEG_KW, CAN_STORE_FLOAT}, @@ -360,8 +371,8 @@ const CanMessageDef MSG_ID_SEND3 = { .dir = CAN_DIR_RX, .dlc = 8, .name = "ID_SEND3", - .symbols = NULL, - .symbol_count = 0, + .symbols = SYM_ID_SEND3, + .symbol_count = (uint8_t)(sizeof(SYM_ID_SEND3)/sizeof(SYM_ID_SEND3[0])), .tx_template = NULL, .send_on_boot = 0, .rx_handler = NULL diff --git a/Core/Inc/Ids/id_504042.h b/Core/Inc/Ids/id_504042.h index 85ac1e6..5288203 100644 --- a/Core/Inc/Ids/id_504042.h +++ b/Core/Inc/Ids/id_504042.h @@ -38,12 +38,12 @@ /* ALL FBKW */ -#define FBKW_FEEDBACK_ZERO 49.559 +#define FBKW_FEEDBACK_ZERO 49.571 #define FBKW_FEEDBACK_MIN -5.367 #define FBKW_FEEDBACK_MAX 21.27 -#define FBKW_FEEDBACK_IC_DT 14 +#define FBKW_FEEDBACK_IC_DT 19 /* CAN DEFINITIONS */ #define CAN_BAUDRATE 500 diff --git a/Core/Inc/id.h b/Core/Inc/id.h index c559723..54ec1de 100644 --- a/Core/Inc/id.h +++ b/Core/Inc/id.h @@ -40,12 +40,12 @@ /* ALL FBKW */ -#define FBKW_FEEDBACK_ZERO 49.559 +#define FBKW_FEEDBACK_ZERO 49.571 //49.559 #define FBKW_FEEDBACK_MIN -5.367 #define FBKW_FEEDBACK_MAX 21.27 -#define FBKW_FEEDBACK_IC_DT 14 +#define FBKW_FEEDBACK_IC_DT 19 /* CAN DEFINITIONS */ #define CAN_BAUDRATE 500 diff --git a/Core/Src/fuel_map.c b/Core/Src/fuel_map.c index 01c8e2f..7f28987 100644 --- a/Core/Src/fuel_map.c +++ b/Core/Src/fuel_map.c @@ -97,6 +97,11 @@ void init_FuelMap(float *PHIAD) { void Timer1_FM_ISR(){ phi_tick_1khz(&s_phi_state, &s_phi_cal); } + +void TW_FM_ISR(uint8_t currentTooth, uint32_t ic){ + //phi_tick_1khz(&s_phi_state, &s_phi_cal); +} + extern float forceTemp; float FM_GET_PHIAD(float RPM, float ME, float Temp) { s_rpm = RPM; diff --git a/Core/Src/toothed_wheel.c b/Core/Src/toothed_wheel.c index d2a4f34..f019b7d 100644 --- a/Core/Src/toothed_wheel.c +++ b/Core/Src/toothed_wheel.c @@ -278,7 +278,7 @@ float fb_2; uint8_t fb_lock; float new_fb = 0.0; float lpf_fb = 1.0;//0.4 -uint8_t feedbackMT = 0; +uint8_t feedbackNew = 1; uint8_t valid = 0; float ckp_rpm = 0.0; @@ -302,11 +302,12 @@ void TW_CALC_FBKW_FEEDBACK(){ return; } } - uint8_t index0 = FBKW_FEEDBACK_ZERO / TW_TOOTH_ALPHA; + float eff_ckp_zero = FBKW_GET_CKP_OFFSET(); + uint8_t index0 = eff_ckp_zero / TW_TOOTH_ALPHA - 1; teethCKP = index; - uint8_t nteeths = TW_PERCYL_TEETH - index0 + 1; + uint8_t nteeths = TW_PERCYL_TEETH - index0; uint32_t diffCKP_e = IC_CKP_Real >= edgeBuf[index0-1].ic ? IC_CKP_Real - edgeBuf[index0-1].ic : IC_CKP_Real - (-edgeBuf[index0-1].ic - 0xffffffff); /*float b_a = 1.0*FBKW_FEEDBACK_ZERO / TW_TOOTH_ALPHA - index0; //remaining angle @@ -328,9 +329,7 @@ void TW_CALC_FBKW_FEEDBACK(){ //uint32_t diffCKP = toNext ? diffCKP_2 : diffCKP_1; uint32_t diffCKP = IC_CKP_Real >= edgeBuf[index0].ic ? IC_CKP_Real - edgeBuf[index0].ic : IC_CKP_Real - (-edgeBuf[index0].ic - 0xffffffff); - if(feedbackMT){ - diffCKP = IC_CKP2 >= IC_MT ? IC_CKP2 - IC_MT : IC_CKP2 - (IC_MT - 0xffffffff); - } + //uint32_t diffCKP = IC_CKP2 >= IC_MT ? IC_CKP2 - IC_MT : IC_CKP2 - (IC_MT - 0xffffffff); float diffbot = 0; float difftop = 0; @@ -340,13 +339,18 @@ void TW_CALC_FBKW_FEEDBACK(){ } }else{ if(count_CKP > 1){ - if(feedbackMT){ - fb_2 = MT_RPM * 6.0 * diffCKP / refClock - dFi; + if(feedbackNew){ + uint8_t now = 24; + uint32_t P_seg = edgeBuf[now].ic - edgeBuf[index0].ic; // wrap-safe by uint32 modular arithmetic + uint32_t T_lag = edgeBuf[now].ic - IC_CKP_Real; // same + + float fraction_before_engine_pulse = 1 - (1.0f*T_lag / P_seg); + fb_2 = fraction_before_engine_pulse * (now - index0) * TW_TOOTH_ALPHA; }else{ - fb_2 = avgrpm * 6.0 * diffCKP_e / refClock + (index0-1) * TW_TOOTH_ALPHA - dFi; + fb_2 = avgrpm * 6.0 * diffCKP_e / refClock + (index0-1) * TW_TOOTH_ALPHA; } - fb_2 -= FBKW_FEEDBACK_ZERO; + fb_2 += feedbackNew*45 - eff_ckp_zero; if(fb_2 > FBKW_FEEDBACK_MIN && fb_2 < FBKW_FEEDBACK_MAX){ //si esta entre margenes new_fb = fb_2; @@ -359,14 +363,19 @@ void TW_CALC_FBKW_FEEDBACK(){ }else{ //feedbackfirst = RPM * 6.0 * diffCKP / refClock; - if(feedbackMT){ - fb_1 = ckp_rpm * 6.0 * diffCKP_e / refClock + (index0-1) * TW_TOOTH_ALPHA - dFi; + if(feedbackNew){ + uint8_t now = 24; + uint32_t P_seg = edgeBuf[now].ic - edgeBuf[index0].ic; // wrap-safe by uint32 modular arithmetic + uint32_t T_lag = edgeBuf[now].ic - IC_CKP_Real; // same + + float fraction_before_engine_pulse = 1 - (1.0f*T_lag / P_seg); + fb_1 = fraction_before_engine_pulse * (now - index0) * TW_TOOTH_ALPHA; }else{ //fb_1 = (toNext ? -1 : 1) * edgeBuf[index + toNext].rpm * 6.0 * diffCKP / refClock + (teethCKP + toNext) * TW_TOOTH_ALPHA - dFi; - fb_1 = avgrpm * 6.0 * diffCKP_e / refClock + (index0-1) * TW_TOOTH_ALPHA - dFi; + fb_1 = avgrpm * 6.0 * diffCKP_e / refClock + (index0-1) * TW_TOOTH_ALPHA; } - fb_1 -= FBKW_FEEDBACK_ZERO; + fb_1 += feedbackNew*45 - eff_ckp_zero; if(fb_1 > FBKW_FEEDBACK_MIN && fb_1 < FBKW_FEEDBACK_MAX){ new_fb = fb_1; @@ -805,7 +814,7 @@ void TW_Service(){ TM_UPDATE_INJECTION_RPM_AND_ALPHA(real_eoi, actual_phi1, IC_EOI, IC_TEIN); inj_rpm_pending = 0; } - if(ckp_process_pending){ + if(ckp_process_pending && (currentTooth == 24)){ // esto es para la 504042, estaria bien revisar el resto... TW_CALC_FBKW_FEEDBACK(); }