aplm8000sdk/src/modules/location/lct_toa.c

// 秒寻科技
// TOA模块
// zt
// 2023-02-24

#include "../../includes/includes.h"
#include "./lct_public.h"

// 根据距离计算所有基站的第一径偏移量
// byFirstPathFlag:第一径类型，计算径或者跟踪径
// pstFirstPathList:第一径队列
// pstMeanLctCoordList:平滑定位坐标队列
// dwAheadOffset:基站上一次偏移量
// pdwBtsFirstPahtOffset:基站第一径偏移量
void lct_toa_cal_bts_first_path_index_offset(uint8 byFirstPathFlag, LCT_FIRSTPATH_LIST_T *pstFirstPathList, LCT_COORDSEL_MEAN_COORD_T *pstMeanLctCoordList, uint32 dwAheadOffset, uint32 *pdwBtsFirstPahtOffset)
{
    uint8 byMeanCoordNum = pstMeanLctCoordList->byListLen;
    COORD_T *pstAheadCoord = NULL; // 上一个定位点坐标

    uint8 byFirstPathNum = pstFirstPathList->byBtsNum;
    LCT_FIRSTPATH_T *pstFirstPathData = NULL;
    COORD_T stBtsCoord = {0};            // 基站坐标
    uint32 dwFirstPahtIndexDistance = 0; // 计算绝对距离计算的第一径索引
    uint32 dwOffset = 0;                 // 基站偏移量
    uint32 dwOffsetGap = 0;              // 前后两次偏移量间隔
    uint32 dwOffsetSum = 0;              // 累计偏移量
    uint32 dwAvarageOffset = 0;          // 平均偏移量
    uint8 byCounter = 0;

    // 参数容错
    if (byMeanCoordNum <= 0 || byFirstPathNum <= 0)
    {
        return;
    }
    pstAheadCoord = &pstMeanLctCoordList->astCoordList[byMeanCoordNum - 1];

    // 遍历第一径队列，计算第一径偏移量综合
    for (uint8 i = 0; i < byFirstPathNum; i++)
    {
        pstFirstPathData = &pstFirstPathList->astFirstPath[i];

        // 基于距离计算第一径索引值
        stBtsCoord.dwX = pstFirstPathData->dwCoordX;
        stBtsCoord.dwY = pstFirstPathData->dwCoordY;
        stBtsCoord.dwZ = pstFirstPathData->dwCoordZ;
        dwFirstPahtIndexDistance = lct_toa_cal_one_bts_first_path_value(&stBtsCoord, pstAheadCoord);

        // 手机到基站间的距离距离有效
        if (byFirstPathFlag == CAL_PATH)
        {
            dwOffset = pstFirstPathData->dwCalPathIndex - dwFirstPahtIndexDistance; // 信号第一径与距离第一径之间的偏移量
        }
        else
        {
            dwOffset = pstFirstPathData->dwTrackPathIndex - dwFirstPahtIndexDistance; // 信号第一径与距离第一径之间的偏移量
        }

        // 基站上一次偏移量有效
        if (dwAheadOffset != 0)
        {
            dwOffsetGap = abs(dwOffset - dwAheadOffset);
            if (dwOffsetGap <= PUB_LCT_FP_OFFSET_VALID_THRES)
            {
                dwOffsetSum += dwOffset;
                byCounter++;
            }
        }
        else
        {
            dwOffsetSum += dwOffset;
            byCounter++;
        }
    }

    // 计算本次平均偏移量
    if (byCounter <= 0)
    {
        dwAvarageOffset = dwAheadOffset;
    }
    else
    {
        dwAvarageOffset = dwOffsetSum / byCounter;
    }

    // 保存偏移量
    if (byMeanCoordNum >= LCT_COORDSEL_MEAN_COORD_LIST_LEN)
    {
        // 移动队列
        if (pstMeanLctCoordList->adwFirstPathOffset[byMeanCoordNum - 1] <= 0)
        {
            pstMeanLctCoordList->adwFirstPathOffset[byMeanCoordNum - 1] = dwAvarageOffset;
        }
        else
        {
            memcpy(pstMeanLctCoordList->adwFirstPathOffset, &pstMeanLctCoordList->adwFirstPathOffset[1], sizeof(uint32) * (byMeanCoordNum - 1));
            pstMeanLctCoordList->adwFirstPathOffset[byMeanCoordNum - 1] = dwAvarageOffset;
        }
    }
    else
    {
        pstMeanLctCoordList->adwFirstPathOffset[byMeanCoordNum - 1] = dwAvarageOffset;
    }

    // 计算最终偏移量
    dwOffsetSum = 0;
    for (uint8 j = 0; j < byMeanCoordNum; j++)
    {
        dwOffsetSum += pstMeanLctCoordList->adwFirstPathOffset[j];
    }
    dwAvarageOffset = dwOffsetSum / byMeanCoordNum;
    *pdwBtsFirstPahtOffset = dwAvarageOffset;

    // printf("final offset:%d\r\n", dwAvarageOffset);

    return;
}

// 基于距离计算计算的第一径绝对值
// pstBtsCoord:基站坐标
// pstAheadCoord:上一个定位点坐标
uint32 lct_toa_cal_one_bts_first_path_value(COORD_T *pstBtsCoord, COORD_T *pstAheadCoord)
{
    uint32 dwFirstPathIndex = 0;
    flt32 fDistance = 0;
    uint32 dwXX = (pstBtsCoord->dwX - pstAheadCoord->dwX) * (pstBtsCoord->dwX - pstAheadCoord->dwX);
    uint32 dwYY = (pstBtsCoord->dwY - pstAheadCoord->dwY) * (pstBtsCoord->dwY - pstAheadCoord->dwY);
    uint32 dwZZ = (pstBtsCoord->dwZ - pstAheadCoord->dwZ) * (pstBtsCoord->dwZ - pstAheadCoord->dwZ);
    uint32 dwSum = dwXX + dwYY + dwZZ;
    fDistance = sqrt((flt64)dwSum) / 100;

    dwFirstPathIndex = (uint32)(fDistance * PUB_SIGNAL_SAMPLE_RATIO / PUB_AUDIO_SPEED);

    return dwFirstPathIndex;
}