small_water_meter_4g_src/Users/tdc_tof/pipe_apsr.c

#include "stdint.h"
#include "board.h"
#include "tdc_tof.h"

#if 0
float bilinear_quad_apsr(float WatSpdLvl, float WatTemp)
{
    uint16_t row;
    uint16_t start, end, mid;
    float TmpFloat;
    float *CoePtr;

    start = 0;
    end   = pTdcItf->row_num;
    while((start+1)<end){
        mid = (start + end)>>1;
        TmpFloat = pTdcItf->row_temp_ary[mid];

        if (TmpFloat<=WatTemp){
            start = mid;
        } else {
            end = mid;
        }
    }
    row = start;

    start = row*pTdcItf->col_num;
    end   = start+pTdcItf->col_num;
    while((start+1)<end){
        mid = (start + end)>>1;
        TmpFloat = pTdcItf->quad_cell_ary[mid].SpdLvl;

        // for ascending array
        if (TmpFloat<=WatSpdLvl){
            start = mid;
        } else {
            end = mid;
        }
    }
    CoePtr = (float *)(pTdcItf->quad_cell_ary[start].Coe);

    TmpFloat  = *CoePtr++;
    TmpFloat += (*CoePtr++)*WatSpdLvl;
    TmpFloat += (*CoePtr++)*WatTemp;
    TmpFloat += (*CoePtr)*WatSpdLvl*WatTemp;
		
		LOG("WatSpdLvl=%.6f, SpdLvl=%.6f, m0=%.6f, m1=%.6f, m2=%.6f, m3=%.6f\n",WatSpdLvl,pTdcItf->quad_cell_ary[mid].SpdLvl,pTdcItf->quad_cell_ary[start].Coe[0],pTdcItf->quad_cell_ary[start].Coe[1],pTdcItf->quad_cell_ary[start].Coe[2],pTdcItf->quad_cell_ary[start].Coe[3]);
    return(TmpFloat);
}
#endif

#if 0
float bilinear_quad_apsr(float WatSpdLvl, float WatTemp)
{
    // 空指针安全判断
    if (pTdcItf == NULL) {
        LOG("[Error] pTdcItf is NULL\n");
        return 0.0f;
    }

    uint16_t start, end, mid;
    uint16_t row, row1;
    uint16_t col, col1;
    float t0, t1, s0, s1;
    float wt, ws;
    float F00, F01, F10, F11;
    float Res;

    //------------------------------------------------------------------------
    // 1. 二分查找：温度所在行（使用 pTdcItf->row_num）
    //------------------------------------------------------------------------
    start = 0;
    end   = pTdcItf->row_num;  // 这里替换成你的结构体成员

    while ((start + 1) < end) {
        mid = (start + end) >> 1;
        if (pTdcItf->row_temp_ary[mid] <= WatTemp)
            start = mid;
        else
            end = mid;
    }
    row  = start;
    row1 = (row >= pTdcItf->row_num - 1) ? row : row + 1;  // 边界保护

    //------------------------------------------------------------------------
    // 2. 二分查找：速度所在列
    //------------------------------------------------------------------------
    start = row * pTdcItf->col_num;
    end   = start + pTdcItf->col_num;

    while ((start + 1) < end) {
        mid = (start + end) >> 1;
        if (pTdcItf->quad_cell_ary[mid].SpdLvl <= WatSpdLvl)
            start = mid;
        else
            end = mid;
    }
    col  = start % pTdcItf->col_num;
    col1 = (col >= pTdcItf->col_num - 1) ? col : col + 1;

    //------------------------------------------------------------------------
    // 3. 计算权重（关键：解决来回跳、抖动）
    //------------------------------------------------------------------------
    t0 = pTdcItf->row_temp_ary[row];
    t1 = pTdcItf->row_temp_ary[row1];
    s0 = pTdcItf->quad_cell_ary[row  * pTdcItf->col_num + col ].SpdLvl;
    s1 = pTdcItf->quad_cell_ary[row  * pTdcItf->col_num + col1].SpdLvl;

    // 计算 0~1 之间的连续权重，彻底避免索引跳变
    wt = (t1 == t0) ? 0.0f : (WatTemp - t0) / (t1 - t0);
    ws = (s1 == s0) ? 0.0f : (WatSpdLvl - s0) / (s1 - s0);

    //------------------------------------------------------------------------
    // 4. 取出 4 个点的系数
    //------------------------------------------------------------------------
    const float *C00 = pTdcItf->quad_cell_ary[row  * pTdcItf->col_num + col ].Coe;
    const float *C01 = pTdcItf->quad_cell_ary[row  * pTdcItf->col_num + col1].Coe;
    const float *C10 = pTdcItf->quad_cell_ary[row1 * pTdcItf->col_num + col ].Coe;
    const float *C11 = pTdcItf->quad_cell_ary[row1 * pTdcItf->col_num + col1].Coe;

    //------------------------------------------------------------------------
    // 5. 4 个点分别计算结果
    //------------------------------------------------------------------------
    F00 = C00[0] + C00[1] * WatSpdLvl + C00[2] * WatTemp + C00[3] * WatSpdLvl * WatTemp;
    F01 = C01[0] + C01[1] * WatSpdLvl + C01[2] * WatTemp + C01[3] * WatSpdLvl * WatTemp;
    F10 = C10[0] + C10[1] * WatSpdLvl + C10[2] * WatTemp + C10[3] * WatSpdLvl * WatTemp;
    F11 = C11[0] + C11[1] * WatSpdLvl + C11[2] * WatTemp + C11[3] * WatSpdLvl * WatTemp;

    //------------------------------------------------------------------------
    // 6. 真正双线性插值（平滑输出，不跳变）
    //------------------------------------------------------------------------
    Res = (1.0f - ws) * (1.0f - wt) * F00
        + (        ws) * (1.0f - wt) * F01
        + (1.0f - ws) * (        wt) * F10
        + (        ws) * (        wt) * F11;

    //------------------------------------------------------------------------
    // 日志（修复了越界问题）
    //------------------------------------------------------------------------
    /*
    LOG("WatSpdLvl=%.6f, SpdLvl=%.6f, m0=%.6f, m1=%.6f, m2=%.6f, m3=%.6f\n",
        WatSpdLvl,
        pTdcItf->quad_cell_ary[start].SpdLvl,
        C00[0], C00[1], C00[2], C00[3]);
    LOG("F00=%.6f, F01=%.6f, F10=%.6f, F11=%.6f, ws=%.6f, wt=%.6f, Res=%.6f\n",
        F00,F01,F10,F11,ws,wt,Res);
    */
    return Res;
}
#endif

// ===================== 配置常量 =====================
#define TEMP_POINT_NUM     8    // 温度点数量
#define FLOW_POINT_NUM     20   // 流速点数量
#define TEMP_START_IDX     0    // 温度起始索引
#define FLOW_START_IDX     8    // 流速起始索引

// ===================== 双线性插值（带权重）最终函数 =====================
float bilinear_quad_apsr(float WatSpdLvl, float WatTemp)
{
    // 1. 边界限制
    if (WatTemp < 5.0f)   WatTemp = 5.0f;
    if (WatTemp > 50.0f)  WatTemp = 50.0f;
    if (WatSpdLvl < 0.0f) WatSpdLvl = 0.0f;

    uint16_t start, end, mid;
    uint16_t t0_idx, t1_idx;    // 温度两个点
    uint16_t f0_idx, f1_idx;    // 流速两个点
    float t0, t1;               // 温度值
    float f0, f1;               // 流速等级值
    float wt, ws;               // 权重（0~1）
    float temp_comp0, temp_comp1;
    float flow_comp0, flow_comp1;
    float F00, F01, F10, F11;
    float result;

    //------------------------------------------------------------------------
    // 2. 二分查找温度区间
    //------------------------------------------------------------------------
    start = TEMP_START_IDX;
    end   = TEMP_START_IDX + TEMP_POINT_NUM;
    while ((start + 1) < end) {
        mid = (start + end) >> 1;
        if (Quad_Cell_Ary[mid].value <= WatTemp)
            start = mid;
        else
            end = mid;
    }
    t0_idx = start;
    t1_idx = (t0_idx >= TEMP_POINT_NUM - 1) ? t0_idx : t0_idx + 1;

    //------------------------------------------------------------------------
    // 3. 二分查找流速区间
    //------------------------------------------------------------------------
    start = FLOW_START_IDX;
    end   = FLOW_START_IDX + FLOW_POINT_NUM;
    while ((start + 1) < end) {
        mid = (start + end) >> 1;
        if (Quad_Cell_Ary[mid].value <= WatSpdLvl)
            start = mid;
        else
            end = mid;
    }
    f0_idx = start;
    f1_idx = (f0_idx >= FLOW_START_IDX + FLOW_POINT_NUM - 1) ? f0_idx : f0_idx + 1;

    //------------------------------------------------------------------------
    // 4. 读取两个温度、两个流速的点数值
    //------------------------------------------------------------------------
    t0 = Quad_Cell_Ary[t0_idx].value;
    t1 = Quad_Cell_Ary[t1_idx].value;
    f0 = Quad_Cell_Ary[f0_idx].value;
    f1 = Quad_Cell_Ary[f1_idx].value;

    //------------------------------------------------------------------------
    // 5. 计算权重（核心：平滑插值）
    //------------------------------------------------------------------------
    wt = (t1 == t0) ? 0.0f : (WatTemp - t0) / (t1 - t0);
    ws = (f1 == f0) ? 0.0f : (WatSpdLvl - f0) / (f1 - f0);

    //------------------------------------------------------------------------
    // 6. 计算温度补偿 & 流速补偿
    //------------------------------------------------------------------------
    // 温度补偿
    temp_comp0 = Quad_Cell_Ary[t0_idx].coe.m + Quad_Cell_Ary[t0_idx].coe.k;
    temp_comp1 = Quad_Cell_Ary[t1_idx].coe.m + Quad_Cell_Ary[t1_idx].coe.k;

    // 流速补偿：如果全是0 → 不补偿，直接=1.0f
    if (f0 == 0.0f && f1 == 0.0f) {
        flow_comp0 = 1.0f;
        flow_comp1 = 1.0f;
    } else {
        flow_comp0 = Quad_Cell_Ary[f0_idx].coe.m + Quad_Cell_Ary[f0_idx].coe.k;
        flow_comp1 = Quad_Cell_Ary[f1_idx].coe.m + Quad_Cell_Ary[f1_idx].coe.k;
    }

    //------------------------------------------------------------------------
    // 7. 四个顶点 = 温度补偿 * 流速补偿
    //------------------------------------------------------------------------
    F00 = temp_comp0 * flow_comp0;
    F01 = temp_comp0 * flow_comp1;
    F10 = temp_comp1 * flow_comp0;
    F11 = temp_comp1 * flow_comp1;

    //------------------------------------------------------------------------
    // 8. 双线性插值（带权重，平滑输出）
    //------------------------------------------------------------------------
    result = (1.0f - ws) * (1.0f - wt) * F00
           + (        ws) * (1.0f - wt) * F01
           + (1.0f - ws) * (        wt) * F10
           + (        ws) * (        wt) * F11;
		LOG("F00=%.6f,F01=%.6f,F10=%.6f,F11=%.6f,result=%.6f\r\n",F00,F01,F10,F11,result);
    return result;
}