Skip to content
C
7.7 KB
Raw Blame
#include "GT7DerivedMetrics.h"
#include <math.h>

void GT7AbsEstimator::update(float vehicleKmh,float brakePercent,
                             const float wheelAngularSpeed[4],
                             const float tyreRadius[4],
                             uint32_t now)
{
    if(vehicleKmh < minSpeedKmh || brakePercent < minBrakePercent){
        active = (now-lastTriggerTime)<sustainMs;
        return;
    }

    float maxSlip=0;
    for(int i=0;i<4;i++){
        float wheelKmh=fabs(wheelAngularSpeed[i])*tyreRadius[i]*3.6f;
        float slip=vehicleKmh-wheelKmh;
        if(slip>maxSlip) maxSlip=slip;
    }

    if(maxSlip>triggerDiffKmh){
        lastTriggerTime=now;
    }

    active=(maxSlip>triggerDiffKmh)||((now-lastTriggerTime)<sustainMs);
}

bool GT7AbsEstimator::isActive() const { return active; }

float GT7AbsEstimator::intensity(uint32_t now) const{
    if(!active) return 0;
    return 70.0f+sinf(now/20.0f)*20.0f;
}

void GT7FuelEstimator::reset(){
    lapStartFuel=-1.0f;
    lastFuel=-1.0f;
    lastLap=-1;
    avgFuelPerLap=0.0f;
    remaining=-1.0f;
    validSamples=0;
}

void GT7FuelEstimator::update(float fuelLevel, int currentLap) {
    if (currentLap < 0 || fuelLevel < 0.0f) return;

    if (lastLap < 0) {
        lastLap = currentLap;
        lapStartFuel = fuelLevel;
        lastFuel = fuelLevel;
        return;
    }

    if (currentLap < lastLap) {
        reset();
        lastLap = currentLap;
        lapStartFuel = fuelLevel;
        lastFuel = fuelLevel;
        return;
    }

    // 同一圈內進站加油,重新設定該圈的油量基準
    if (fuelLevel > lastFuel + 0.5f) {
        lapStartFuel = fuelLevel;
    }

    if (currentLap > lastLap) {
        const float used = lapStartFuel - fuelLevel;

        // 略過起跑時 0 → 1 的假圈數切換
        // 也排除過小或明顯異常的油耗樣本
        const bool completedRealLap =
            lastLap >= 1 &&
            currentLap == lastLap + 1 &&
            used >= 0.2f &&
            used <= 50.0f;

        if (completedRealLap) {
            if (validSamples == 0) {
                avgFuelPerLap = used;
            } else {
                avgFuelPerLap =
                    avgFuelPerLap * 0.7f +
                    used * 0.3f;
            }

            ++validSamples;
        }

        lastLap = currentLap;
        lapStartFuel = fuelLevel;
    }

    lastFuel = fuelLevel;

    remaining =
        (validSamples > 0 && avgFuelPerLap >= 0.2f)
            ? fuelLevel / avgFuelPerLap
            : -1.0f;
}

float GT7FuelEstimator::remainingLaps() const {
    return remaining;
}

float GT7DeltaTracker::distanceSquared(
    float x1, float y1, float z1,
    float x2, float y2, float z2)
{
    const float dx = x1 - x2;
    const float dy = y1 - y2;
    const float dz = z1 - z2;

    return dx * dx + dy * dy + dz * dz;
}

int GT7DeltaTracker::findNearestReference(
    float x, float y, float z)
{
    if(referenceCount <= 0){
        return -1;
    }

    int bestIndex = lastReferenceIndex;
    float bestDistance = INFINITY;

    // 正常情況只往目前位置附近搜尋,
    // 避免髮夾彎或賽道交叉時跳到錯誤路段。
    int start = lastReferenceIndex - 5;
    int end = lastReferenceIndex + 30;

    if(start < 0) start = 0;
    if(end >= referenceCount) end = referenceCount - 1;

    for(int i = start; i <= end; ++i){
        const float d = distanceSquared(
            x, y, z,
            reference[i].x,
            reference[i].y,
            reference[i].z
        );

        if(d < bestDistance){
            bestDistance = d;
            bestIndex = i;
        }
    }

    // 區域搜尋結果距離超過 50 公尺,
    // 可能是剛開始、封包中斷或索引跑掉,改做全域搜尋。
    constexpr float GLOBAL_SEARCH_DISTANCE_SQ = 50.0f * 50.0f;

    if(bestDistance > GLOBAL_SEARCH_DISTANCE_SQ){
        bestDistance = INFINITY;

        for(int i = 0; i < referenceCount; ++i){
            const float d = distanceSquared(
                x, y, z,
                reference[i].x,
                reference[i].y,
                reference[i].z
            );

            if(d < bestDistance){
                bestDistance = d;
                bestIndex = i;
            }
        }
    }

    return bestIndex;
}

void GT7DeltaTracker::reset()
{
    referenceCount = 0;
    currentCount = 0;
    sampleCounter = 0;

    lastReferenceIndex = 0;
    previousReferenceIndex = -1;

    bestLapTime = 0;

    currentDelta = 0.0f;
    projectedDelta = 0.0f;

    previousDelta = 0.0f;
    deltaSlope = 0.0f;

    deltaValid = false;
}

void GT7DeltaTracker::beginLap()
{
    currentCount = 0;
    sampleCounter = 0;

    lastReferenceIndex = 0;
    previousReferenceIndex = -1;

    currentDelta = 0.0f;
    projectedDelta = 0.0f;

    previousDelta = 0.0f;
    deltaSlope = 0.0f;

    deltaValid = false;
}

void GT7DeltaTracker::sample(
    float x,
    float y,
    float z,
    uint32_t lapTime)
{
    // 不需要每個 UDP 封包都存。
    ++sampleCounter;

    if(sampleCounter < SAMPLE_DIVIDER){
        return;
    }

    sampleCounter = 0;

    if(currentCount < MAX_POINTS){
        DeltaPoint &point = current[currentCount];

        point.x = x;
        point.y = y;
        point.z = z;
        point.lapTime = lapTime;

        ++currentCount;
    }

    // 第一圈還沒有參考資料,只負責記錄。
    if(referenceCount <= 1){
        deltaValid = false;
        return;
    }

    const int referenceIndex = findNearestReference(x, y, z);

    if(referenceIndex < 0){
        deltaValid = false;
        return;
    }

    lastReferenceIndex = referenceIndex;

    currentDelta =
        static_cast<float>(
            static_cast<int32_t>(lapTime) -
            static_cast<int32_t>(
                reference[referenceIndex].lapTime
            )
        ) / 1000.0f;

    deltaValid = true;

    /*
     * Delta P:
     *
     * 觀察 Delta 在參考圈索引上的變化速度,
     * 再把趨勢外推到終點。
     */
    if(previousReferenceIndex >= 0 &&
       referenceIndex > previousReferenceIndex)
    {
        const int indexDifference =
            referenceIndex - previousReferenceIndex;

        const float newSlope =
            (currentDelta - previousDelta) /
            static_cast<float>(indexDifference);

        // 平滑斜率,避免單一封包造成預測值劇烈跳動。
        deltaSlope =
            deltaSlope * 0.85f +
            newSlope * 0.15f;
    }

    const int remainingPoints =
        referenceCount - 1 - referenceIndex;

    projectedDelta =
        currentDelta +
        deltaSlope * static_cast<float>(remainingPoints);

    // 防止定位錯誤時出現非常離譜的數字。
    if(projectedDelta > 60.0f){
        projectedDelta = 60.0f;
    }else if(projectedDelta < -60.0f){
        projectedDelta = -60.0f;
    }

    previousDelta = currentDelta;
    previousReferenceIndex = referenceIndex;
}

void GT7DeltaTracker::endLap(uint32_t lapTime)
{
    // 圈無效、資料太少或時間不合理,不更新參考圈。
    if(lapTime == 0 || currentCount < 10){
        return;
    }

    const bool firstReference =
        referenceCount == 0;

    const bool newBestLap =
        bestLapTime == 0 ||
        lapTime < bestLapTime;

    if(firstReference || newBestLap){
        referenceCount = currentCount;

        if(referenceCount > MAX_POINTS){
            referenceCount = MAX_POINTS;
        }

        for(int i = 0; i < referenceCount; ++i){
            reference[i] = current[i];
        }

        bestLapTime = lapTime;
    }
}

float GT7DeltaTracker::delta() const
{
    return deltaValid ? currentDelta : NAN;
}

float GT7DeltaTracker::deltaP() const
{
    return deltaValid ? projectedDelta : NAN;
}

void GT7DerivedMetrics::reset(){
    fuel.reset();
    delta.reset();
}