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

added oem ckp acquisition, better for acceleration artifacts. Also added proper ckp offset calculation. id 700 sended an offset and a eeprom commit to pump. Other cfgs should be tested for this

Browse Source
This commit is contained in:
LucianoDev
2026-05-05 12:55:22 +02:00
parent d88629a5cc
commit e0c001e3b6
11 changed files with 207 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
Core/Advance_Control/FBKW.c
View File

@@ -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;
}

3
Core/Advance_Control/FBKW.h
View File

@@ -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_ */

15
Core/Advance_Control/cal_tables_rom.c
View File

@@ -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) */

93
Core/Advance_Control/ckp_acquisition.c Normal file
View File

@@ -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;
}

41
Core/Advance_Control/ckp_acquisition.h Normal file
View File

@@ -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 <stdint.h>
/* 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_ */

5
Core/Advance_Control/pwm.h
View File

@@ -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 */

17
Core/CAN_Libs/can_db.c
View File

@@ -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

4
Core/Inc/Ids/id_504042.h
View File

@@ -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

4
Core/Inc/id.h
View File

@@ -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

5
Core/Src/fuel_map.c
View File

@@ -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;

39
Core/Src/toothed_wheel.c
View File

@@ -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();
}