120 lines
4.6 KiB
C
120 lines
4.6 KiB
C
/*
|
||
* fuel_map.c — T06215 variant
|
||
*
|
||
* Thin wrapper that exposes the legacy host-app API
|
||
* (init_FuelMap / FM_GET_PHIAD) on top of the reverse-engineered
|
||
* T06215 phi pipeline. The original float trilinear interpolation
|
||
* (5 fuel maps × RPM × ME plus a cosine empirical correction) has
|
||
* been removed — the phi pipeline is now the sole angle-calculation
|
||
* source.
|
||
*
|
||
* Mapping:
|
||
* FM_GET_PHIAD(RPM, ME, Temp) -> phi_outputs_t.angle_accumulator
|
||
* (RW52, "0052 accumulator")
|
||
* scaled to float degrees (/ 85.333).
|
||
*
|
||
* Getter scale factors mirror example_interface_getters.txt:
|
||
* RPM raw = real_rpm × 8.388 (CF_TMS)
|
||
* ME raw = real_me × 32 (CF_ME)
|
||
* Temp raw = real_°C × 16 + 4368 (CF_T_M, CF_T_N)
|
||
*/
|
||
|
||
#include "fuel_map.h"
|
||
#include "phi.h"
|
||
#include <stdint.h>
|
||
#include <stddef.h>
|
||
|
||
#define CF_TMS 8.388f
|
||
#define CF_ME 32.0f
|
||
#define CF_T_M 16.0f
|
||
#define CF_T_N 4368.0f
|
||
#define CF_KW 85.3333f
|
||
#define ANGLE_DEG_PER_RAW (1.0f / 85.333f)
|
||
|
||
/* Per-call input cache. FM_GET_PHIAD's float args land here so the
|
||
* nullary phi getters can read them. */
|
||
static float s_rpm = 0.0f;
|
||
static float s_me = 0.0f;
|
||
static float s_temp = 0.0f;
|
||
|
||
/* Encapsulated phi runtime — opaque to the caller. */
|
||
static phi_state_t s_phi_state;
|
||
static phi_cal_t s_phi_cal;
|
||
static phi_input_getters_t s_phi_getters;
|
||
static phi_outputs_t s_phi_out;
|
||
static float *s_phiad_out = NULL;
|
||
|
||
/* ──── Getters (nullary; T06215 vtable shape) ────────────────────────── */
|
||
|
||
static uint16_t get_rpm(void) { return (uint16_t)(s_rpm * CF_TMS); }
|
||
static int16_t get_inj_qty_demand(void) { return (int16_t) (s_me * CF_ME); }
|
||
static int16_t get_temperature(void) { return (int16_t) (s_temp * CF_T_M + CF_T_N); }
|
||
|
||
/* angle-decrease CAN command. Default 0 matches sim_t06215_sweep.c.
|
||
* To wire this to a host-app global, replace the body with e.g.:
|
||
* extern float B_PHIAD;
|
||
* return (int16_t)(B_PHIAD * CF_KW);
|
||
*/
|
||
static int16_t get_angle_dec_cmd(void) { return 0; }
|
||
|
||
/* Steady-state default: baseline tracks current RPM, so Δ-rpm = 0 and
|
||
* the accel-comp branch contributes nothing. Override if modelling
|
||
* transients. */
|
||
static int16_t get_rpm_baseline(void) { return (int16_t)(s_rpm * CF_TMS); }
|
||
|
||
static uint16_t get_rwc2(void) { return 0; } /* slow timing scratch */
|
||
static uint8_t get_reset_gate_0226(void) { return 0xFF; } /* accel-comp enabled */
|
||
static int16_t get_dphi(void) { return 0; }
|
||
static uint8_t get_scratch_0103(void) { return 0xFF; } /* matches sim_t06215_sweep.c */
|
||
static uint8_t get_scratch_0108(void) { return 0xFF; }
|
||
|
||
/* ──── Public API ─────────────────────────────────────────────────────── */
|
||
|
||
void init_FuelMap(float *PHIAD) {
|
||
s_phiad_out = PHIAD;
|
||
|
||
s_phi_getters.get_rpm = get_rpm;
|
||
s_phi_getters.get_inj_qty_demand = get_inj_qty_demand;
|
||
s_phi_getters.get_temperature = get_temperature;
|
||
s_phi_getters.get_angle_dec_cmd = get_angle_dec_cmd;
|
||
//s_phi_getters.get_rpm_baseline = get_rpm_baseline;
|
||
s_phi_getters.get_rwc2 = get_rwc2;
|
||
s_phi_getters.get_reset_gate_0226 = get_reset_gate_0226;
|
||
s_phi_getters.get_dphi = get_dphi;
|
||
s_phi_getters.get_scratch_0103 = get_scratch_0103;
|
||
s_phi_getters.get_scratch_0108 = get_scratch_0108;
|
||
|
||
s_phi_cal = phi_t06215_cal;
|
||
phi_init(&s_phi_state, &s_phi_cal, &s_phi_getters);
|
||
|
||
/* Pre-latch to skip the cranking-phase ramp (mirrors
|
||
* sim_t06215_sweep.c:237-238). With this in place, the very first
|
||
* phi_service call enters FUN_62a2 in Phase 2 and tein_valve_fault_guard
|
||
* holds at cal_48 from boot. */
|
||
s_phi_state.rt.tein_valve_fault_guard = s_phi_cal.cal_48;
|
||
s_phi_state.rt.scratch_010f = s_phi_state.rt.scratch_0108;
|
||
}
|
||
void Timer1_FM_ISR(){
|
||
phi_tick_1khz(&s_phi_state, &s_phi_cal);
|
||
}
|
||
|
||
void TW_FM_ISR(uint8_t currentTooth, uint32_t ic){
|
||
phi_tooth_isr(&s_phi_state, &s_phi_cal, currentTooth, (uint16_t)(ic >> 4));
|
||
}
|
||
|
||
extern float forceTemp;
|
||
float FM_GET_PHIAD(float RPM, float ME, float Temp) {
|
||
s_rpm = RPM;
|
||
s_me = ME;
|
||
s_temp = Temp;
|
||
|
||
phi_service(&s_phi_state, &s_phi_cal, &s_phi_out);
|
||
|
||
float Z = (float)s_phi_out.angle_accumulator * ANGLE_DEG_PER_RAW;
|
||
Z = s_me ? Z : 0; // el terrano se apagaba
|
||
|
||
if (s_phiad_out) *s_phiad_out = Z;
|
||
return Z;
|
||
}
|
||
|