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 */ .can_dckp_offset_bias = -427, /* cal+0x050 @ 0x9C28 = 0xFE55 */
.ckp_modulus = 7680, /* cal+0x0A0 @ 0x9C78 = 0x1E00 (90°) */ .ckp_modulus = 7680, /* cal+0x0A0 @ 0x9C78 = 0x1E00 (90°) */
/* CKP process-tooth derivation (FUN_6c70 — feeds get_ckp_process_tooth()). /* CKP process-tooth derivation (FUN_7293 — feeds get_ckp_process_tooth()).
* Read direct from rom_eeprom_dump_0000-9FFF_504042.bin at RWA4=0x9BB8 * Read direct from 424026.bin at RWA4=0x9BD8 + offset; default-family
* + offset; default-family analogs are at cal+0x120/0x0A3/0x0A4. */ * analogs are at cal+0x120/0x0A3/0x0A4. */
.ckp_advance_per_tick = 1707, /* cal+0x140 @ 0x9CF8 = 0x06AB (= 20° per tick; .ckp_advance_per_tick = 1707, /* cal+0x12C @ 0x9D04 = 0x06AB (= 20° per tick;
* t06211 cal+0x124, T06215 cal+0x12C, default cal+0x120 = 1536 = 18°) */ * t06211 cal+0x124, T06235 cal+0x140, default cal+0x120 = 1536 = 18°) */
.ckp_seg_wrap_threshold = 29, /* cal+0x09F @ 0x9C57 (default cal+0x0A3) */ .ckp_seg_wrap_threshold = 29, /* cal+0x09D @ 0x9C75 (default cal+0x0A3) */
.ckp_teeth_per_seg = 26, /* cal+0x0A0 @ 0x9C58 (default cal+0x0A4; same value across all variants) */ .ckp_teeth_per_seg = 26, /* cal+0x09E @ 0x9C76 (default cal+0x0A4; same value across all variants) */
/* Recovery / sustained-error machinery */ /* Recovery / sustained-error machinery */
.pi_state_c2_reload = 100, /* cal+0x114 (ROM 0x9CEC = 0x0064) */ .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) * @file cal_tables_rom.h (families/T06215/compact_src)
* @brief Extern decls for the ROM-decoded T06235 calibration. * @brief Extern decls for the ROM-decoded T06215 calibration.
* All declarations live in pwm.h; this file just re-exports the enum * All declarations live in pwm.h; this file just re-exports the enum
* and array symbols for the compact_src/pwm.c translation unit. * and array symbols for the compact_src/pwm.c translation unit.
*/ */
#ifndef CAL_TABLES_ROM_T06235_COMPACT_H #ifndef CAL_TABLES_ROM_T06211_COMPACT_H
#define CAL_TABLES_ROM_T06235_COMPACT_H #define CAL_TABLES_ROM_T06211_COMPACT_H
#include "pwm.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 * Mirrors ROM Stage-1 caller @ 0x6330 (LCALL FUN_70d8) → FUN_70d8 @ 0x70d8
* Author: herli * 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 * Stage-1 caller sequence (T06215 ROM 0x62b0..0x6332):
* acquisition path. See ckp_acquisition.h for the data-flow summary. * 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 "ckp_acquisition.h"
#include "pwm.h" /* pwm_cal_rom + shra16 */ #include "pwm.h"
#include "ee_manager.h" #include "ee_manager.h"
/* Inputs */ /* Inputs */
int16_t B_CKP_OFFSET = 0; int16_t B_CKP_OFFSET = 0;
uint8_t commitCKP_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; int16_t CKP_RAM_TRIM_0430 = 0;
/* Outputs */ /* Outputs */
int16_t dCKP_OFFSET = 0; int16_t dCKP_OFFSET = 0;
int16_t CKP_ZERO_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) static void ckp_recompute_zero_offset(void)
{ {
int16_t anchor = (int16_t)((int16_t)pwm_cal_rom.ckp_zero_anchor int16_t anchor = (int16_t)((int16_t)pwm_cal_rom.ckp_zero_anchor
+ CKP_RAM_TRIM_0430); + 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 int16_t v = (int16_t)(anchor
- dCKP_OFFSET - dCKP_OFFSET
- pwm_cal_rom.can_dckp_offset_bias - 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; 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 < 0) v = (int16_t)(v + mod);
while (v > mod) v = (int16_t)(v - mod); while (v > mod) v = (int16_t)(v - mod);
CKP_ZERO_OFFSET = v; 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) int16_t get_ckp_zero(void)
{ {
static uint8_t initialized = 0; static uint8_t initialized = 0;
@@ -77,10 +69,6 @@ int16_t get_ckp_zero(void)
if (halved != biased_now) { if (halved != biased_now) {
dCKP_OFFSET = (int16_t)(halved dCKP_OFFSET = (int16_t)(halved
- pwm_cal_rom.can_dckp_offset_bias); - 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(); ckp_recompute_zero_offset();
initialized = 1; initialized = 1;
} else if (!initialized) { } else if (!initialized) {
@@ -91,27 +79,28 @@ int16_t get_ckp_zero(void)
return CKP_ZERO_OFFSET; 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 * Translates t06215 dissasembly.txt:21346–21364 (the second half of
* after the R8F clamp; only the R90 byte path is reproduced here since * FUN_7293 after the R8F clamp; only the R90 byte path is reproduced
* R8F is computed by the caller from word[0x152] high byte directly): * here since R8F is computed by the caller from word[0x152] high byte
* directly):
* *
* 6c8b LD RW1C, [0x152] ; CKP_ZERO_OFFSET * 72ae LD RW1C, [0x152] ; CKP_ZERO_OFFSET
* 6c90 ADD RW1C, cal+0x140 ; ckp_advance_per_tick * 72b3 ADD RW1C, cal+0x12C ; ckp_advance_per_tick
* 6c95 INCB R1D ; high byte += 1 * 72b8 INCB R1D ; high byte += 1
* 6c97 CMPB R1D, cal+0x09F ; ckp_seg_wrap_threshold (=29) * 72ba CMPB R1D, cal+0x09D ; ckp_seg_wrap_threshold (=29)
* 6c9c JLE LAB_6ca5 * 72bf JLE LAB_72c8
* 6c9e SUB RW1C, cal+0x0A2 ; ckp_modulus (low byte=0 → R1D-=30) * 72c1 SUB RW1C, cal+0x0A0 ; ckp_modulus (low byte=0 → R1D-=30)
* 6ca3 SJMP LAB_6cae * 72c6 SJMP LAB_72d1
* LAB_6ca5: * LAB_72c8:
* 6ca5 CMPB R1D, cal+0x0A0 ; ckp_teeth_per_seg (=26) * 72c8 CMPB R1D, cal+0x09E ; ckp_teeth_per_seg (=26)
* 6caa JLE LAB_6cae * 72cd JLE LAB_72d1
* 6cac CLRB R1D * 72cf CLRB R1D
* LAB_6cae: * LAB_72d1:
* 6cae STB R1D, R90 * 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 * zero low byte, so the high byte (R1D) decreases by exactly
* (ckp_modulus >> 8) = 30 with no borrow into the low byte. We * (ckp_modulus >> 8) = 30 with no borrow into the low byte. We
* therefore reproduce the byte-level effect directly without keeping * 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 * CKP-zero acquisition chain for T06215. Mirrors ROM
* Author: herli * FUN_624a-class Stage-1 caller @ 0x6330 (LCALL FUN_70d8) → FUN_70d8 @ 0x70d8.
* *
* Translates the CAN-supplied raw CKP offset (B_CKP_OFFSET, RW134 / * Body copied verbatim from variant/T06235 per the pillar-port copy gate
* DAT_0134) into the ROM-equivalent runtime delta dCKP_OFFSET (DAT_0434) * (docs/pillar-functions.md). Only cal-field bindings change.
* and then into the canonical CKP zero-crossing offset CKP_ZERO_OFFSET *
* (DAT_0152). Mirrors ROM FUN_55e0 @ 0x55e0 → FUN_6a4b @ 0x6a4b. * See docs/algorithm-ckp-zero-acquisition.md for the data-flow summary.
*/ */
#ifndef PWM_T06215_CKP_ACQUISITION_H
#ifndef ADVANCE_CONTROL_CKP_ACQUISITION_H_ #define PWM_T06215_CKP_ACQUISITION_H
#define ADVANCE_CONTROL_CKP_ACQUISITION_H_
#include <stdint.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) */ extern uint8_t commitCKP_offset; /* unused for now (commit gate hook) */
/* Internal RAM trims (DAT_0430 word, DAT_0404 signed byte). No runtime /* Internal RAM trims (DAT_0430 word, DAT_0404 signed byte). No runtime
* writer was found in this ROM/car (project memory: * writer was found in this ROM/car; sit at 0 unless a future step wires
* project_t06235_ckp_processing.md), so they sit at 0 unless a future * them up. Exposed for diagnostic poking. */
* step wires them up. Exposed for diagnostic poking. */
extern int16_t CKP_RAM_TRIM_0430; extern int16_t CKP_RAM_TRIM_0430;
extern int8_t CKP_RAM_TRIM_0404; extern int8_t CKP_RAM_TRIM_0404;
/* Outputs */ /* Outputs */
extern int16_t dCKP_OFFSET; /* DAT_0434 — output of FUN_55e0 */ extern int16_t dCKP_OFFSET; /* DAT_0434 — output of Stage 1 */
extern int16_t CKP_ZERO_OFFSET; /* DAT_0152 — output of FUN_6a4b */ 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); 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); 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 target_5e_min_clamp; /* cal+0x12A */
int16_t can_aux_12e_max; /* cal+0x002 */ 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 /* CKP-zero acquisition (FUN_70d8 chain) cal constants — see
* docs/algorithm-ckp-zero-acquisition.md. T06215 cal slot offsets * docs/algorithm-ckp-zero-acquisition.md. T06215 cal slot offsets
* differ from T06235 (T06235 has +0x02 lower-region shift). */ * differ from T06235 (T06235 has +0x02 lower-region shift). */
int16_t ckp_zero_anchor; /* cal+0x04E — FUN_70d8 additive anchor */ 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 can_dckp_offset_bias; /* cal+0x050 — Stage-1/Stage-2 bias */
int16_t ckp_modulus; /* cal+0x0A0 — FUN_70d8 modulo wrap */ int16_t ckp_modulus; /* cal+0x0A0 — FUN_70d8 modulo wrap (also reused
>>>>>>> 0ad144c (terrano setup, not working) * 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 */ /* Recovery / sustained-error machinery */
int16_t pi_state_c2_reload; /* cal+0x114 — reload value for pi_state_c2 on latch */ 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_SEND3 0x700
#define ID_SEND4 0x300 #define ID_SEND4 0x300
#elif defined(T06215) || defined(T06211) #elif defined(T06215) || defined(T06211)
#define ID_EMPF1 0x112 #define ID_EMPF1 0x113
#define ID_EMPF2 0x512 #define ID_EMPF2 0x513
#define ID_EMPF3 0x712 #define ID_EMPF3 0x713
#define ID_SEND1 0x100 #define ID_SEND1 0x100
#define ID_SEND2 0x500 #define ID_SEND2 0x500
#define ID_SEND3 0x700 #define ID_SEND3 0x700
@@ -54,11 +54,12 @@
/* ===== Extern RAM variables declared elsewhere ===== */ /* ===== Extern RAM variables declared elsewhere ===== */
extern uint16_t BitStatus; extern uint16_t BitStatus;
extern float PHI_AD; extern float PHI_AD;
extern float RPM; extern float RPM, MT_offset_RPM, MT_RPM;
extern float Temp; extern float Temp;
extern float ME, MEPI, B_FB_KW, dFi, B_PHIAD; extern float ME, MEPI, B_FB_KW, dFi, B_PHIAD;
extern int16_t B_FB_NW; extern int16_t B_FB_NW;
extern int16_t B_CKP_OFFSET; extern int16_t B_CKP_OFFSET;
extern int8_t s_dfi_code;
extern uint8_t cilCount, safetySHUTOFF; extern uint8_t cilCount, safetySHUTOFF;
extern uint8_t inj_mode, request_syncout_activation, commitCKP_offset; 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_DC = { "%", 100.0f/4095.0f, 0.0f };
static const CanScaleDef SCALE_TEIN = { "uS", 1.0f/2.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 = { "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 }; 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 }, { 0x7800, &T_ein, CAN_SYM_UX, &SCALE_TEIN , CAN_STORE_FLOAT },
{ 0x4201, &PSG_Voltage, CAN_SYM_UX, &SCALE_VOLTAGE, 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 }, { 0x4400, &ME, CAN_SYM_UX, &SCALE_ME_RAM, CAN_STORE_FLOAT },
{ 0x4800, &PHI_AD, CAN_SYM_SX, &SCALE_DEG_KW, 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 }, { 0x4C00, &actual_phi1, CAN_SYM_SX, &SCALE_DEG_KW, CAN_STORE_FLOAT },
@@ -327,7 +333,7 @@ static CanSymbolDef SYM_ID_SEND4[] = {
#endif #endif
static CanSymbolDef SYM_ID_SEND3[] = { 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}, { "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}, { "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_frequency = 0;
volatile float MT_RPM = 0; volatile float MT_RPM = 0;
volatile float MT_offset_RPM = 0;
//volatile float next_MT_RPM = 0; //volatile float next_MT_RPM = 0;
volatile float last_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 uint8_t SYNCOUT_clear = 0;
volatile uint32_t RPM_Difference; volatile uint32_t RPM_Difference;
volatile uint32_t RPM_offset_Difference;
volatile uint8_t startedEngine = 0; volatile uint8_t startedEngine = 0;
volatile uint8_t count_CKP = 0; volatile uint8_t count_CKP = 0;
@@ -191,7 +194,13 @@ void TW_TEETH_CAPTURE(){
INJ_EVAL_EOI_COMPENSATION(); INJ_EVAL_EOI_COMPENSATION();
} }
INJ_EVAL_END(); 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); UpdateEdgeBuffer(IC_RPM_Val2, TEETH_RPM, currentTooth, isInjecting);
if(!hasInjected){ if(!hasInjected){
@@ -227,6 +236,7 @@ void TW_TEETH_CAPTURE(){
TW_FM_ISR(currentTooth,IC_RPM_Val2 ); TW_FM_ISR(currentTooth,IC_RPM_Val2 );
hasCapturedTeeth = 1; hasCapturedTeeth = 1;
if(hasInjectionEndedFlag == 1){ if(hasInjectionEndedFlag == 1){
@@ -395,7 +405,6 @@ void TW_CALC_FBKW_FEEDBACK(){
diffbot = FBKW_FEEDBACK_MIN - fb_1; diffbot = FBKW_FEEDBACK_MIN - fb_1;
difftop = fb_1 - FBKW_FEEDBACK_MAX; 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_FEEDBACK -= lpf_fb*(FBKW_FEEDBACK - new_fb - fbkw_offset);
FBKW_CKP_ISR();
ckp_process_pending=0; ckp_process_pending=0;
} }