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

424026 pwm tested, with 700 ckp offset and pid working like oem. Also added 0138 rpm baseline calculation and it is tested, it can be merged into other projects for phi accel compensation. Research would be needed if our accel comp would make it worse on top of oem

Browse Source
This commit is contained in:
LucianoDev
2026-05-05 22:30:59 +02:00
parent de8a09361b
commit 98a835026e
7 changed files with 130 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
Core/Advance_Control/cal_tables_rom.c
View File

@@ -110,13 +110,13 @@ const pwm_calibration_t pwm_cal_rom = {
.can_dckp_offset_bias = -427, /* cal+0x050 @ 0x9C28 = 0xFE55 */
.ckp_modulus = 7680, /* cal+0x0A0 @ 0x9C78 = 0x1E00 (90°) */
/* CKP process-tooth derivation (FUN_6c70 — feeds get_ckp_process_tooth()).
* Read direct from rom_eeprom_dump_0000-9FFF_504042.bin at RWA4=0x9BB8
* + offset; default-family analogs are at cal+0x120/0x0A3/0x0A4. */
.ckp_advance_per_tick = 1707, /* cal+0x140 @ 0x9CF8 = 0x06AB (= 20° per tick;
* t06211 cal+0x124, T06215 cal+0x12C, default cal+0x120 = 1536 = 18°) */
.ckp_seg_wrap_threshold = 29, /* cal+0x09F @ 0x9C57 (default cal+0x0A3) */
.ckp_teeth_per_seg = 26, /* cal+0x0A0 @ 0x9C58 (default cal+0x0A4; same value across all variants) */
/* CKP process-tooth derivation (FUN_7293 — feeds get_ckp_process_tooth()).
* Read direct from 424026.bin at RWA4=0x9BD8 + offset; default-family
* analogs are at cal+0x120/0x0A3/0x0A4. */
.ckp_advance_per_tick = 1707, /* cal+0x12C @ 0x9D04 = 0x06AB (= 20° per tick;
* t06211 cal+0x124, T06235 cal+0x140, default cal+0x120 = 1536 = 18°) */
.ckp_seg_wrap_threshold = 29, /* cal+0x09D @ 0x9C75 (default cal+0x0A3) */
.ckp_teeth_per_seg = 26, /* cal+0x09E @ 0x9C76 (default cal+0x0A4; same value across all variants) */
/* Recovery / sustained-error machinery */
.pi_state_c2_reload = 100, /* cal+0x114 (ROM 0x9CEC = 0x0064) */

10
Core/Advance_Control/cal_tables_rom.h
View File

@@ -1,12 +1,12 @@
/**
* @file cal_tables_rom.h (variant/T06235/compact_src)
* @brief Extern decls for the ROM-decoded T06235 calibration.
* @file cal_tables_rom.h (families/T06215/compact_src)
* @brief Extern decls for the ROM-decoded T06215 calibration.
* All declarations live in pwm.h; this file just re-exports the enum
* and array symbols for the compact_src/pwm.c translation unit.
*/
#ifndef CAL_TABLES_ROM_T06235_COMPACT_H
#define CAL_TABLES_ROM_T06235_COMPACT_H
#ifndef CAL_TABLES_ROM_T06211_COMPACT_H
#define CAL_TABLES_ROM_T06211_COMPACT_H
#include "pwm.h"
#endif /* CAL_TABLES_ROM_T06235_COMPACT_H */
#endif /* CAL_TABLES_ROM_T06211_COMPACT_H */

99
Core/Advance_Control/ckp_acquisition.c
View File

@@ -1,71 +1,63 @@
/*
* ckp_acquisition.c
* ckp_acquisition.c (variant/T06215/compact_src)
*
* Created on: May 5, 2026
* Author: herli
* Mirrors ROM Stage-1 caller @ 0x6330 (LCALL FUN_70d8) → FUN_70d8 @ 0x70d8
* for the CKP-zero acquisition path. Body verbatim from T06235; only the
* cal-field bindings (ckp_zero_anchor / can_dckp_offset_bias / ckp_modulus)
* carry T06215-specific cal slots — see pwm_addr_map.h.
*
* Mirrors ROM FUN_55e0 @ 0x55e0 → FUN_6a4b @ 0x6a4b for the CKP-zero
* acquisition path. See ckp_acquisition.h for the data-flow summary.
* Stage-1 caller sequence (T06215 ROM 0x62b0..0x6332):
* RW1E = dCKP_OFFSET + cal+0x50 ; biased current
* RW1C = SHRA(B_CKP_OFFSET, 1) ; signed half
* if RW1C != RW1E:
* DAT_01e7 |= 0x80
* dCKP_OFFSET = RW1C - cal+0x50
* (flash commit gated by AD_resultlo == cal+0xCD)
* DAT_01e7 &= 0x7F
* LCALL FUN_70d8
*
* Stage-2 body (FUN_70d8 @ 0x70d8..0x7128):
* RW1E = cal+0x4e + DAT_0430
* v = RW1E - DAT_0434 - cal+0x50 + (int8_t)DAT_0404
* while v < 0 : v += cal+0xa0 ; signed JGE
* while v > cal+0xa0 : v -= cal+0xa0 ; signed JLE
* DAT_0152 = v
* DAT_0150 = cal+0x4c - RW1E
*/
#include "ckp_acquisition.h"
#include "pwm.h" /* pwm_cal_rom + shra16 */
#include "pwm.h"
#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. */
/* RAM trims */
int16_t CKP_RAM_TRIM_0430 = 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)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;
@@ -77,10 +69,6 @@ int16_t get_ckp_zero(void)
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) {
@@ -91,27 +79,28 @@ int16_t get_ckp_zero(void)
return CKP_ZERO_OFFSET;
}
/* FUN_6c70 @ 0x6c70 — process-tooth derivation (R90 byte store path).
/* FUN_7293 @ 0x7293 — process-tooth derivation (R90 byte store path).
*
* Translates fulldissasembly.txt:20453–20471 (the second half of FUN_6c70
* after the R8F clamp; only the R90 byte path is reproduced here since
* R8F is computed by the caller from word[0x152] high byte directly):
* Translates t06215 dissasembly.txt:21346–21364 (the second half of
* FUN_7293 after the R8F clamp; only the R90 byte path is reproduced
* here since R8F is computed by the caller from word[0x152] high byte
* directly):
*
* 6c8b LD RW1C, [0x152] ; CKP_ZERO_OFFSET
* 6c90 ADD RW1C, cal+0x140 ; ckp_advance_per_tick
* 6c95 INCB R1D ; high byte += 1
* 6c97 CMPB R1D, cal+0x09F ; ckp_seg_wrap_threshold (=29)
* 6c9c JLE LAB_6ca5
* 6c9e SUB RW1C, cal+0x0A2 ; ckp_modulus (low byte=0 → R1D-=30)
* 6ca3 SJMP LAB_6cae
* LAB_6ca5:
* 6ca5 CMPB R1D, cal+0x0A0 ; ckp_teeth_per_seg (=26)
* 6caa JLE LAB_6cae
* 6cac CLRB R1D
* LAB_6cae:
* 6cae STB R1D, R90
* 72ae LD RW1C, [0x152] ; CKP_ZERO_OFFSET
* 72b3 ADD RW1C, cal+0x12C ; ckp_advance_per_tick
* 72b8 INCB R1D ; high byte += 1
* 72ba CMPB R1D, cal+0x09D ; ckp_seg_wrap_threshold (=29)
* 72bf JLE LAB_72c8
* 72c1 SUB RW1C, cal+0x0A0 ; ckp_modulus (low byte=0 → R1D-=30)
* 72c6 SJMP LAB_72d1
* LAB_72c8:
* 72c8 CMPB R1D, cal+0x09E ; ckp_teeth_per_seg (=26)
* 72cd JLE LAB_72d1
* 72cf CLRB R1D
* LAB_72d1:
* 72d1 STB R1D, R90
*
* The word SUB at 6c9e operates on RW1C, but cal+0x0A2 = 0x1E00 has a
* The word SUB at 72c1 operates on RW1C, but cal+0x0A0 = 0x1E00 has a
* zero low byte, so the high byte (R1D) decreases by exactly
* (ckp_modulus >> 8) = 30 with no borrow into the low byte. We
* therefore reproduce the byte-level effect directly without keeping

56
Core/Advance_Control/ckp_acquisition.h
View File

@@ -1,17 +1,16 @@
/*
* ckp_acquisition.h
* ckp_acquisition.h (variant/T06215/compact_src)
*
* Created on: May 5, 2026
* Author: herli
* CKP-zero acquisition chain for T06215. Mirrors ROM
* FUN_624a-class Stage-1 caller @ 0x6330 (LCALL FUN_70d8) → FUN_70d8 @ 0x70d8.
*
* 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.
* Body copied verbatim from variant/T06235 per the pillar-port copy gate
* (docs/pillar-functions.md). Only cal-field bindings change.
*
* See docs/algorithm-ckp-zero-acquisition.md for the data-flow summary.
*/
#ifndef ADVANCE_CONTROL_CKP_ACQUISITION_H_
#define ADVANCE_CONTROL_CKP_ACQUISITION_H_
#ifndef PWM_T06215_CKP_ACQUISITION_H
#define PWM_T06215_CKP_ACQUISITION_H
#include <stdint.h>
@@ -20,23 +19,36 @@ 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. */
* writer was found in this ROM/car; 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 */
extern int16_t dCKP_OFFSET; /* DAT_0434 — output of Stage 1 */
extern int16_t CKP_ZERO_OFFSET; /* DAT_0152 — output of Stage 2 (FUN_70d8) */
/* 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);
/* Get the next process-tooth index — mirrors the byte stored to R90
* in ROM FUN_7293 @ 0x7293 (analog of default-family FUN_87ea @ 0x87ea).
*
* The ROM routine advances the CKP zero-crossing accumulator by a fixed
* per-tick angular increment, then renormalizes the high byte (segment-
* tooth view) into the valid tooth range with two wrap branches:
*
* advanced = CKP_ZERO_OFFSET + ckp_advance_per_tick (16-bit ADD)
* tooth = (advanced >> 8) + 1 (byte INC of high)
* if (tooth > ckp_seg_wrap_threshold) (T06215: 29)
* tooth -= (ckp_modulus >> 8) (= 30 — angular wrap)
* else if (tooth > ckp_teeth_per_seg) (T06215: 26)
* tooth = 0 (hard reset)
* return tooth;
*
* No side-effects on CKP_ZERO_OFFSET or dCKP_OFFSET — the ROM SUB on
* RW1C is local to the function frame; only the byte (R90) escapes via
* the store at 0x72d1.
*/
uint8_t get_ckp_process_tooth(void);
#endif /* ADVANCE_CONTROL_CKP_ACQUISITION_H_ */
#endif /* PWM_T06215_CKP_ACQUISITION_H */

40
Core/Advance_Control/pwm.h
View File

@@ -192,34 +192,28 @@ struct pwm_calibration {
int16_t target_5e_min_clamp; /* cal+0x12A */
int16_t can_aux_12e_max; /* cal+0x002 */
<<<<<<< HEAD
/* 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 (also reused
* by the FUN_6c70 process-tooth derivation
* below; high byte = ckp_modulus>>8 = 30). */
/* CKP process-tooth derivation (FUN_6c70 @ 0x6c70 — analog of default-
* family FUN_87ea @ 0x87ea). Computes the byte stored to R90: a
* segment-counter view of (CKP_ZERO_OFFSET + advance) with wrap rules.
* Consumed by get_ckp_process_tooth() in ckp_acquisition.c. */
int16_t ckp_advance_per_tick; /* cal+0x140 — angular advance added to
* word[0x152] before tooth derivation
* (T06235: 1707 = 20° at 85.33/°). */
int16_t ckp_seg_wrap_threshold; /* cal+0x09F — tooth-counter test:
* if tooth > threshold → tooth -= 30
* (T06235: 29). */
int16_t ckp_teeth_per_seg; /* cal+0x0A0 — per-segment clamp:
* if tooth > teeth_per_seg → tooth = 0
* (T06235: 26 teeth / 90° segment). */
=======
/* CKP-zero acquisition (FUN_70d8 chain) cal constants — see
* docs/algorithm-ckp-zero-acquisition.md. T06215 cal slot offsets
* differ from T06235 (T06235 has +0x02 lower-region shift). */
int16_t ckp_zero_anchor; /* cal+0x04E — FUN_70d8 additive anchor */
int16_t can_dckp_offset_bias; /* cal+0x050 — Stage-1/Stage-2 bias */
int16_t ckp_modulus; /* cal+0x0A0 — FUN_70d8 modulo wrap */
>>>>>>> 0ad144c (terrano setup, not working)
int16_t ckp_modulus; /* cal+0x0A0 — FUN_70d8 modulo wrap (also reused
* by the FUN_7293 process-tooth derivation
* below; high byte = ckp_modulus>>8 = 30). */
/* CKP process-tooth derivation (FUN_7293 @ 0x7293 — analog of default-
* family FUN_87ea @ 0x87ea). Computes the byte stored to R90: a
* segment-counter view of (CKP_ZERO_OFFSET + advance) with wrap rules.
* Consumed by get_ckp_process_tooth() in ckp_acquisition.c. */
int16_t ckp_advance_per_tick; /* cal+0x12C — angular advance added to
* word[0x152] before tooth derivation
* (T06215: 1707 = 20° at 85.33/°). */
int16_t ckp_seg_wrap_threshold; /* cal+0x09D — tooth-counter test:
* if tooth > threshold → tooth -= 30
* (T06215: 29). */
int16_t ckp_teeth_per_seg; /* cal+0x09E — per-segment clamp:
* if tooth > teeth_per_seg → tooth = 0
* (T06215: 26 teeth / 90° segment). */
/* Recovery / sustained-error machinery */
int16_t pi_state_c2_reload; /* cal+0x114 — reload value for pi_state_c2 on latch */

16
Core/CAN_Libs/can_db.c
View File

@@ -24,9 +24,9 @@
#define ID_SEND3 0x700
#define ID_SEND4 0x300
#elif defined(T06215) || defined(T06211)
#define ID_EMPF1 0x112
#define ID_EMPF2 0x512
#define ID_EMPF3 0x712
#define ID_EMPF1 0x113
#define ID_EMPF2 0x513
#define ID_EMPF3 0x713
#define ID_SEND1 0x100
#define ID_SEND2 0x500
#define ID_SEND3 0x700
@@ -54,11 +54,12 @@
/* ===== Extern RAM variables declared elsewhere ===== */
extern uint16_t BitStatus;
extern float PHI_AD;
extern float RPM;
extern float RPM, MT_offset_RPM, MT_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 int8_t s_dfi_code;
extern uint8_t cilCount, safetySHUTOFF;
extern uint8_t inj_mode, request_syncout_activation, commitCKP_offset;
@@ -97,6 +98,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_RPM_OUT = { "1/min", 1.0f/8.388f, 0.0f };
static const CanScaleDef SCALE_VOLTAGE= { "V", 0.0185f, -0.05f };
@@ -151,6 +153,10 @@ const CanAddressEntry CAN_ANSWERS[] = {
{ 0x7800, &T_ein, CAN_SYM_UX, &SCALE_TEIN , CAN_STORE_FLOAT },
{ 0x4201, &PSG_Voltage, CAN_SYM_UX, &SCALE_VOLTAGE, CAN_STORE_FLOAT },
{ 0x4000, &MT_RPM, CAN_SYM_UX, &SCALE_RPM_OUT, CAN_STORE_FLOAT },
{ 0x3801, &MT_offset_RPM, CAN_SYM_UX, &SCALE_RPM_OUT, CAN_STORE_FLOAT },
{ 0x4400, &ME, CAN_SYM_UX, &SCALE_ME_RAM, CAN_STORE_FLOAT },
{ 0x4800, &PHI_AD, CAN_SYM_SX, &SCALE_DEG_KW, CAN_STORE_FLOAT },
{ 0x4C00, &actual_phi1, CAN_SYM_SX, &SCALE_DEG_KW, CAN_STORE_FLOAT },
@@ -327,7 +333,7 @@ static CanSymbolDef SYM_ID_SEND4[] = {
#endif
static CanSymbolDef SYM_ID_SEND3[] = {
#if defined(T06235)
#if defined(T06235) || defined(T06215)
{ "CKP_OFFSET", 0, 16, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &B_CKP_OFFSET, NULL, CAN_STORE_S16},
{ "commit", 16, 1, CAN_ENDIAN_INTEL, CAN_SYM_UX, 0,0, &commitCKP_offset, NULL, CAN_STORE_U8},

14
Core/Src/toothed_wheel.c
View File

@@ -33,6 +33,8 @@ volatile float RPM;
volatile float MT_frequency = 0;
volatile float MT_RPM = 0;
volatile float MT_offset_RPM = 0;
//volatile float next_MT_RPM = 0;
volatile float last_MT_RPM = 0;
@@ -46,6 +48,7 @@ volatile float last_TEETHRPM = 0;
volatile uint8_t SYNCOUT_clear = 0;
volatile uint32_t RPM_Difference;
volatile uint32_t RPM_offset_Difference;
volatile uint8_t startedEngine = 0;
volatile uint8_t count_CKP = 0;
@@ -191,7 +194,13 @@ void TW_TEETH_CAPTURE(){
INJ_EVAL_EOI_COMPENSATION();
}
INJ_EVAL_END();
if(currentTooth == 13){
RPM_offset_Difference = IC_RPM_Val2 - edgeBuf[currentTooth].ic;
MT_offset_RPM = fclamp(60.0 * (refClock/RPM_offset_Difference) / CYLINDERS, MIN_RPM, 4000);
if(MT_RPM <15){
MT_offset_RPM = MT_RPM;
}
}
UpdateEdgeBuffer(IC_RPM_Val2, TEETH_RPM, currentTooth, isInjecting);
if(!hasInjected){
@@ -227,6 +236,7 @@ void TW_TEETH_CAPTURE(){
TW_FM_ISR(currentTooth,IC_RPM_Val2 );
hasCapturedTeeth = 1;
if(hasInjectionEndedFlag == 1){
@@ -395,7 +405,6 @@ void TW_CALC_FBKW_FEEDBACK(){
diffbot = FBKW_FEEDBACK_MIN - fb_1;
difftop = fb_1 - FBKW_FEEDBACK_MAX;
}
FBKW_CKP_ISR();
}
@@ -425,6 +434,7 @@ void TW_CALC_FBKW_FEEDBACK(){
}
FBKW_FEEDBACK -= lpf_fb*(FBKW_FEEDBACK - new_fb - fbkw_offset);
FBKW_CKP_ISR();
ckp_process_pending=0;
}