增加基于toa计算第一径偏移量的代码(toa应用的代码还未写)，修改代码中的缺陷(两处检查极小值点的错误)，输出V2.1.03版本。

aplm8000sdk.code-workspace

@@ -44,7 +44,8 @@
             "route_main.h": "c",
             "lct_public.h": "c",
             "imu.h": "c",
-            "lct_prediction.h": "c"
+            "lct_prediction.h": "c",
+            "lct_coordcal_one.h": "c"
         }
     }
 }

src/includes/includes.h

@@ -49,6 +49,7 @@
 #include "../modules/location/lct_prediction.h"   //预测坐标
 #include "../modules/location/lct_coordcal_one.h" //计算坐标(1维和0维)
 #include "../modules/location/lct_coordcal_two.h" //计算坐标(2维)
+#include "../modules/location/lct_toa.h"          //toa模块
 #include "../modules/location/lct_coordsel.h"     //选择坐标
 
 // imu模块

src/modules/location/lct_coordcal.c

@@ -92,14 +92,6 @@ void lct_coordcal_main(PUB_LCT_BTS_LIST_T *pstLctBtsList, PUB_LOCATION_DATA_T *p
     // lct_coordcal_prediction_coord(&stPredictionCoord, &byPredictionFlag, &stAheadPointCoord, dwBuildId, dwLayerId);
     lct_prediction_current_coord_by_history(dwBuildId, dwLayerId, &stPredictionCoord, &byPredictionFlag, &stAheadPointCoord, &gLct_dwSpeedX, &gLct_dwSpeedY, &byChangeXY);
 
-    // // 修正预测坐标
-    // if (byPredictionFlag)
-    // {
-    //     lct_coordcal_check_prediction_coord_by_bts_coord(&stPredictionCoord, astValidCalPathBtsList, byValidCalPathBtsListLen, astValidTrackPathBtsList, byValidTrackPathBtsListLen);
-    //     *pbyPredictionFlag = byPredictionFlag;
-    //     *pstPredictionCoord = stPredictionCoord;
-    // }
-
     // 预测手机相对于基站的移动趋势，1：靠近趋势，2：远离趋势，0：趋势不确定
     lct_prediction_prediction_phone_move_tendency(gLct_astPredictionDirectionDataList);
 
@@ -197,6 +189,8 @@ void lct_coordcal_select_valid_bts_by_fp_and_ampl(PUB_LOCATION_DATA_T *pstLctDat
     uint8 byTrackCounter = 0; // 跟踪径有效基站参数个数
 
     uint8 byMainBtsFindFalg = 0; // 主站标识符
+    uint32 dwCalMainBtsId = 0;   // 计算径主站编号
+    uint32 dwTrackMainBtsId = 0; // 跟踪径主站编号
 
     for (uint8 i = 0; i < byFirstPathListLen; i++)
     {
@@ -310,51 +304,25 @@ void lct_coordcal_select_valid_bts_by_fp_and_ampl(PUB_LOCATION_DATA_T *pstLctDat
     }
     *pbyValidTrackBtsListLen = byTrackCounter;
 
-    // 根据有效计算径和跟踪径修改第一径数据队列
-    for (uint8 i = 0; i < byFirstPathListLen; i++)
+    // 修改第一径数据队列主站标识符
+    if (byCalCounter || byTrackCounter)
     {
-        pststBtsFirstPath = &pstFirstPathList[i];
-        // pststBtsFirstPath->wFrameCalIndex = 0;
-        // pststBtsFirstPath->wFrameTrackIndex = 0;
-        pststBtsFirstPath->byMainFlag = 0;
+        dwCalMainBtsId = pstValidCalBtsList[0].dwBtsId;
+        dwTrackMainBtsId = pstValidTrackBtsList[0].dwBtsId;
 
-        ////检查计算径
-        for (uint8 j = 0; j < byCalCounter; j++)
+        // 遍历队列
+        for (uint8 i = 0; i < byFirstPathListLen; i++)
         {
-            pststTmpFirstPath = &pstValidCalBtsList[j];
-            if (pststBtsFirstPath->dwBtsId == pststTmpFirstPath->dwBtsId)
-            {
-                // pststBtsFirstPath->wFrameCalIndex = pststTmpFirstPath->wFrameCalIndex;
-                if (j == 0)
-                {
-                    *pdwBulidId = pststTmpFirstPath->dwBuildId;
-                    *pdwLayerId = pststTmpFirstPath->dwLayerId;
-
-                    pststBtsFirstPath->byMainFlag = 1;
-                    byMainBtsFindFalg = 1;
-                }
-                break;
-            }
-        }
+            pststBtsFirstPath = &pstFirstPathList[i];
+            pststBtsFirstPath->byMainFlag = 0;
 
-        // 跟踪径
-        for (uint8 k = 0; k < byTrackCounter; k++)
-        {
-            pststTmpFirstPath = &pstValidTrackBtsList[k];
-            if (pststBtsFirstPath->dwBtsId == pststTmpFirstPath->dwBtsId)
+            // 找到主站
+            if (!byMainBtsFindFalg && (pststBtsFirstPath->dwBtsId == dwCalMainBtsId || pststBtsFirstPath->dwBtsId == dwTrackMainBtsId))
             {
-                // pststBtsFirstPath->wFrameTrackIndex = pststTmpFirstPath->wFrameTrackIndex;
-
-                if (k == 0 && !byMainBtsFindFalg)
-                {
-                    *pdwBulidId = pststTmpFirstPath->dwBuildId;
-                    *pdwLayerId = pststTmpFirstPath->dwLayerId;
-
-                    pststBtsFirstPath->byMainFlag = 1;
-                    byMainBtsFindFalg = 1;
-                }
-
-                break;
+                pststBtsFirstPath->byMainFlag = 1;
+                *pdwBulidId = pststBtsFirstPath->dwBuildId;
+                *pdwLayerId = pststBtsFirstPath->dwLayerId;
+                byMainBtsFindFalg = 1;
             }
         }
     }

src/modules/location/lct_coordcal_two.c

@@ -4,8 +4,6 @@
 // 2023-03-25
 
 #include "../../includes/includes.h"
-// #include "../../sysdefine/struct.h"
-// #include "./lct_public.h"
 
 // 导入全局变量
 extern COORD_T gLct_astLocationRefPoint[LCT_REF_POINT_MATRIX_LEN * LCT_REF_POINT_MATRIX_LEN]; // 定位参考点
@@ -78,7 +76,7 @@ void lct_coordcal_two_dimension_location(PUB_LCT_BTS_LIST_T *pstLctBtsList, LCT_
     }
 
     // 二次校验坐标
-    lct_coordcal_dbcheck_minimum_value(byFirstPathFlag, astValidPathBtsList, byValidPathBtsListLen, pstMinmumValueList);
+    lct_coordcal_dbcheck_minimum_value(byFirstPathFlag, pstLctBtsList, pstMinmumValueList);
 
     return;
 }
@@ -648,9 +646,10 @@ void lct_coordcal_sel_minimum_value(flt32 *pfDelayDeviationSumMatrix, uint16 wMa
 }
 
 // 二次校验极小值
-// pstBtsFirstPathList:第一径队列
+// byFirstPathFlag:第一径标识符
+// pstLctBtsList:定额我i基站队列
 // pstMinimumValueList://极小值队列
-void lct_coordcal_dbcheck_minimum_value(uint8 byFirstPathFlag, LCT_FIRSTPATH_T *pstFirstPathList, uint8 byFirstPathListLen, LCT_MINIMUM_VALUE_GROUP_T *pstMinimumValueList)
+void lct_coordcal_dbcheck_minimum_value(uint8 byFirstPathFlag, PUB_LCT_BTS_LIST_T *pstLctBtsList, LCT_MINIMUM_VALUE_GROUP_T *pstMinimumValueList)
 {
     uint8 byMinimumNum = 0;
     LCT_MINIMUM_VALUE_T *pstMiminumValueData = NULL;
@@ -672,9 +671,9 @@ void lct_coordcal_dbcheck_minimum_value(uint8 byFirstPathFlag, LCT_FIRSTPATH_T *
     uint8 byYflag = 0;
 
     ////计算端点坐标的最大值与最小值
-    for (uint8 i = 0; i < byFirstPathListLen; i++)
+    for (uint8 i = 0; i < pstLctBtsList->byBtsNum; i++)
     {
-        dwTemp = pstFirstPathList[i].dwCoordX;
+        dwTemp = pstLctBtsList->astLocationBts[i].dwCoordX;
         if (dwTemp > dwMaxValueX)
         {
             dwMaxValueX = dwTemp;
@@ -685,7 +684,7 @@ void lct_coordcal_dbcheck_minimum_value(uint8 byFirstPathFlag, LCT_FIRSTPATH_T *
             dwMinValueX = dwTemp;
         }
 
-        dwTemp = pstFirstPathList[i].dwCoordY;
+        dwTemp = pstLctBtsList->astLocationBts[i].dwCoordY;
         if (dwTemp > dwMaxValueY)
         {
             dwMaxValueY = dwTemp;
@@ -711,7 +710,7 @@ void lct_coordcal_dbcheck_minimum_value(uint8 byFirstPathFlag, LCT_FIRSTPATH_T *
     }
 
     // 二次校验极小值坐标
-    pstMinimumValueList->byTwoDimensionInsideFlag = 0;
+    pstMinimumValueList->byTwoDimensionInsideFlag = 0; // 极小值点不在二维空间内
     for (uint8 j = 0; j < byMinimumNum; j++)
     {
         if (byFirstPathFlag == CAL_PATH)
@@ -755,8 +754,12 @@ void lct_coordcal_dbcheck_minimum_value(uint8 byFirstPathFlag, LCT_FIRSTPATH_T *
         }
         pstMiminumValueData->stCoord.dwY = dwY;
 
-        // 再次检查坐标是否合法
-        if ((dwX >= (dwMinValueX - 300)) && (dwX <= (dwMaxValueX + 300)) && (dwY >= (dwMinValueY - 300)) && (dwY <= (dwMaxValueY + 300)))
+        // 再次检查坐标是否合法，极小值点要在基站围成的矩形框内部
+        dwMinValueX -= 300;
+        dwMaxValueX += 300;
+        dwMinValueY -= 300;
+        dwMaxValueY += 300;
+        if ((dwX >= dwMinValueX) && (dwX <= dwMaxValueX) && (dwY >= dwMinValueY) && (dwY <= dwMaxValueY))
         {
             if (byFirstPathFlag == CAL_PATH)
             {
@@ -771,7 +774,7 @@ void lct_coordcal_dbcheck_minimum_value(uint8 byFirstPathFlag, LCT_FIRSTPATH_T *
 
             if (!pstMinimumValueList->byTwoDimensionInsideFlag)
             {
-                pstMinimumValueList->byTwoDimensionInsideFlag = 1;
+                pstMinimumValueList->byTwoDimensionInsideFlag = 1; // 极小值点在二维空间内
             }
         }
     }
@@ -811,16 +814,16 @@ void lct_coordcal_two_dimension_check(LCT_MINIMUM_VALUE_GROUP_T *pstMinmumValueG
                 dwGap = pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX - pstAheadCoord->dwX;
                 if (dwGap < -LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES)
                 {
-                    pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX = pstPredictionCoord->dwX;
+                    pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX = 0.5 * pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX + 0.5 * pstPredictionCoord->dwX;
                 }
             }
             // x轴向左移动
             else if (byDirectionX == -1)
             {
-                dwGap = pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX > pstAheadCoord->dwX;
+                dwGap = pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX - pstAheadCoord->dwX;
                 if (dwGap > LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES)
                 {
-                    pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX = pstPredictionCoord->dwX;
+                    pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX = 0.5 * pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX + 0.5 * pstPredictionCoord->dwX;
                 }
             }
         }
@@ -829,19 +832,19 @@ void lct_coordcal_two_dimension_check(LCT_MINIMUM_VALUE_GROUP_T *pstMinmumValueG
             // y轴向上移动
             if (byDirectionY == 1)
             {
-                dwGap = pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY < pstAheadCoord->dwY;
+                dwGap = pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY - pstAheadCoord->dwY;
                 if (dwGap < -LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES)
                 {
-                    pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY = pstPredictionCoord->dwY;
+                    pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY = 0.5 * pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY + 0.5 * pstPredictionCoord->dwY;
                 }
             }
             // y轴向下移动
             else if (byDirectionY == -1)
             {
-                dwGap = pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY > pstAheadCoord->dwY;
+                dwGap = pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY - pstAheadCoord->dwY;
                 if (dwGap > LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES)
                 {
-                    pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY = pstPredictionCoord->dwY;
+                    pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY = 0.5 * pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY + 0.5 * pstPredictionCoord->dwY;
                 }
             }
         }
@@ -855,19 +858,19 @@ void lct_coordcal_two_dimension_check(LCT_MINIMUM_VALUE_GROUP_T *pstMinmumValueG
             // x轴向右移动
             if (byDirectionX == 1)
             {
-                dwGap = pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX < pstAheadCoord->dwX;
+                dwGap = pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX - pstAheadCoord->dwX;
                 if (dwGap < -LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES)
                 {
-                    pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX = pstPredictionCoord->dwX;
+                    pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX = 0.5 * pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX + 0.5 * pstPredictionCoord->dwX;
                 }
             }
             // x轴向左移动
             else if (byDirectionX == -1)
             {
-                dwGap = pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX > pstAheadCoord->dwX;
+                dwGap = pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX - pstAheadCoord->dwX;
                 if (dwGap > LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES)
                 {
-                    pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX = pstPredictionCoord->dwX;
+                    pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX = 0.5 * pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX + 0.5 * pstPredictionCoord->dwX;
                 }
             }
         }
@@ -876,19 +879,19 @@ void lct_coordcal_two_dimension_check(LCT_MINIMUM_VALUE_GROUP_T *pstMinmumValueG
             // y轴向上移动
             if (byDirectionY == 1)
             {
-                dwGap = pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY < pstAheadCoord->dwY;
+                dwGap = pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY - pstAheadCoord->dwY;
                 if (dwGap < -LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES)
                 {
-                    pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY = pstPredictionCoord->dwY;
+                    pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY = 0.5 * pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY + 0.5 * pstPredictionCoord->dwY;
                 }
             }
             // y轴向下移动
             else if (byDirectionY == -1)
             {
-                dwGap = pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY > pstAheadCoord->dwY;
+                dwGap = pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY - pstAheadCoord->dwY;
                 if (dwGap > LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES)
                 {
-                    pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY = pstPredictionCoord->dwY;
+                    pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY = 0.5 * pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY + 0.5 * pstPredictionCoord->dwY;
                 }
             }
         }

src/modules/location/lct_coordcal_two.h

@@ -20,6 +20,6 @@ void lct_coordcal_cal_ref_point_delay(COORD_T *pstRefPointMatrix, uint16 wMatrix
 void lct_coordcal_cal_two_point_transmit_times_by_coord(COORD_T stFirstPoint, COORD_T stSecondPoint, flt32 *pfTimes);
 void lct_coordcal_cal_delay_by_first_path_index(uint32 dwMainBtsIndex, uint32 dwSlaveBtsIndex, flt32 *pfTwoBtsDelay);
 void lct_coordcal_sel_minimum_value(flt32 *pfDelayDeviationSumMatrix, uint16 wMatrixRow, uint16 wMatrixCol, COORD_T *pstRefPointMatrix, uint8 byFirstPathFlag, LCT_MINIMUM_VALUE_GROUP_T *pstMinimumValueList);
-void lct_coordcal_dbcheck_minimum_value(uint8 byFirstPathFlag, LCT_FIRSTPATH_T *pstFirstPathList, uint8 byFirstPathListLen, LCT_MINIMUM_VALUE_GROUP_T *pstMinimumValueList);
+void lct_coordcal_dbcheck_minimum_value(uint8 byFirstPathFlag, PUB_LCT_BTS_LIST_T *pstLctBtsList, LCT_MINIMUM_VALUE_GROUP_T *pstMinimumValueList);
 void lct_coordcal_two_dimension_check(LCT_MINIMUM_VALUE_GROUP_T *pstMinmumValueGroup, COORD_T *pstPredictionCoord, COORD_T *pstAheadCoord, uint8 byChangeXY, int8 byDirectionX, int8 byDirectionY);
 #endif

src/modules/location/lct_main.c

@@ -12,7 +12,8 @@ LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T gLct_astPredictionDirectionDataList[PU
 LCT_COORDSEL_ORIGINAL_COORD_T gLct_stOriginalLctCoordList = {0};                                          // 原始定位坐标队列
 LCT_COORDSEL_MEAN_COORD_T gLct_stMeanLctCoordList = {0};                                                  // 平滑定位坐标队列
 flt32 gLct_afMeanWeight[LCT_COORDSEL_ORIGINAL_COORD_LIST_LEN] = {0.20, 0.30, 0.50};                       // 历史定位坐标权重
-static uint32 gLct_dwPredictionTimes = 0;                                                                 // 预测次数
+uint32 gLct_dwPredictionTimes = 0;                                                                        // 预测次数
+uint32 gLct_dwFpBaseOffset = 0;                                                                           // 第一径基本偏移量
 
 // 导出全局变量
 extern PUB_SYS_STATUS_T gg_stSysStatus; // 系统运行状态
@@ -161,6 +162,9 @@ void lct_main(PUB_LCT_BTS_LIST_T *pstLctBtsList, PUB_LOCATION_DATA_T *pstLctData
                     // 平滑原始坐标
                     lct_main_mean_original_coord(&gLct_stOriginalLctCoordList, gLct_afMeanWeight, &gLct_stMeanLctCoordList, &stMeanCoord);
 
+                    // 计算第一径基本偏移量
+                    lct_toa_cal_bts_first_path_index_offset(stOptimumCoord.bySourceFlag, &gLct_stFirstPathList, &gLct_stMeanLctCoordList, gLct_dwFpBaseOffset, &gLct_dwFpBaseOffset);
+
                     // 根据定位坐标修改手机相对于基站的移动趋势
                     lct_main_update_phone_tendency(&gLct_stMeanLctCoordList, gLct_astPredictionDirectionDataList);
 

src/modules/location/lct_toa.c

@@ -0,0 +1,134 @@
+// 秒寻科技
+// 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;
+}

src/modules/location/lct_toa.h

@@ -0,0 +1,18 @@
+// 秒寻科技
+// TOA模块
+// zt
+// 2023-02-24
+
+#ifndef __LCT_TOA_H__
+#define __LCT_TOA_H__
+
+#include "../../sysdefine/sysdef.h"
+#include "./lct_public.h"
+
+// 供外部使用接口
+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);
+
+// 内部接口
+uint32 lct_toa_cal_one_bts_first_path_value(COORD_T *pstBtsCoord, COORD_T *pstAheadCoord);
+
+#endif

src/sysdefine/struct_lct.h

@@ -21,8 +21,8 @@
 #define LCT_COORDCAL_FIRSTPATH_ADJUST_THRES 150 // 第一径调整门限（100厘米）
 #define LCT_COORDCAL_FIRSTPATH_ADJUST_VALUE 75  // 第一径调整值（50厘米）
 
-#define LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES 120 // 极小值调整门限（120厘米）
-#define LCT_COORDCAL_MINIMUM_VALUE_ADJUST_VALUE 50  // 极小值调整值（50厘米）
+#define LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES 300 // 极小值调整门限（300厘米）
+// #define LCT_COORDCAL_MINIMUM_VALUE_ADJUST_VALUE 50  // 极小值调整值（50厘米）
 
 // 最优定位坐标选择模块
 #define LCT_COORDSEL_PP_TIME_GAP 1        // 两点之间的时长间隔（单位秒）
@@ -275,6 +275,7 @@ typedef struct
     uint32 dwLayerId; // 楼层编号
     uint8 byListLen;
     COORD_T astCoordList[LCT_COORDSEL_MEAN_COORD_LIST_LEN];
+    uint32 adwFirstPathOffset[LCT_COORDSEL_MEAN_COORD_LIST_LEN];
 } LCT_COORDSEL_MEAN_COORD_T;
 
 // 手机与基站间距离趋势(靠近或者远离)

src/sysdefine/sysmacro.h

@@ -14,8 +14,9 @@
 // V2.1.01 2025-04-14 by zt 修改代码缺陷（修复轨迹跳变，修改极小值选择方法、用运动方向二次校验极小值）
 // V2.1.02 2025-04-22 by zt 完善代码(完善手机移动方向预测代码，并将预测方向应用到二维定位中（利用预测方向修订第一径）；
 // 修改信号同步模块中的设计缺陷，解决轨迹跳变问题，改善了定位效果)
+// V2.1.03 2025-04-25 by zt 增加基于toa计算第一径偏移量的代码(toa应用的代码还未写)，修改代码中的缺陷(两处检查极小值点的错误)
 #define PUB_SOFTWARE_VERSION_LEN 7
-#define PUB_SOFTWARE_VERSION "V2.1.02" // 软件版本
+#define PUB_SOFTWARE_VERSION "V2.1.03" // 软件版本
 
 #define PUB_M_PI 3.14159265358979323846
 #define PUB_2PI (2 * 3.14159265358979323846)
@@ -68,6 +69,7 @@
 #define PUBLIC_LCT_MIN_VALUE_FRONT_LEN 1000 // 真内最小值前面的长度，用于选择帧内噪声值
 #define PUBLIC_LCT_CFAR_LEN 160             // cfar判决统计长度(单边)
 #define PUBLIC_LCT_TRACK_WINDOW_WIDTH 280   // 跟踪第一径窗口长度（单边）
+#define PUB_LCT_FP_OFFSET_VALID_THRES 350   // 第一径偏移门限
 
 // 路由模块
 #define PUB_MAC_ADDRESS_LEN 12 // MAC地址字节数

src/sysmain/sysmain.c

@@ -61,14 +61,14 @@ void sysmain_init_lct()
     stFirstPathParam.byNoiseSignalFactor = 7;
     stFirstPathParam.byNoiseLowScale = 25;   // 底噪放大低倍数，放大10倍保存
     stFirstPathParam.byNoiseHightScale = 50; // 底噪放大高倍数，放大10倍保存
-    stFirstPathParam.byCfarLeftFactor = 120; // 120; //update 125 by zt 2025-01-09
+    stFirstPathParam.byCfarLeftFactor = 120; // CFAR左侧门限
     stFirstPathParam.bySincFactor = 94;      // 95;
     stFirstPathParam.byFirstPathContactFactor = 75;
     stFirstPathParam.wFirstPathContactDistance = 1200; // 8米
 
     // 跟踪径
-    stFirstPathParam.wFirstPathTrackWindowWidth = 320;     // 280;     // 跟踪径搜索范围
-    stFirstPathParam.wFirstPathTrackPeakGuardWidth = 80;   // 160;  // 跟踪保护贷款
+    stFirstPathParam.wFirstPathTrackWindowWidth = 320;     // 280; // 跟踪径搜索范围
+    stFirstPathParam.wFirstPathTrackPeakGuardWidth = 80;   // 160; // 跟踪保护贷款
     stFirstPathParam.byFirstPathTrackPeakGainFactor = 120; // 跟踪补偿因子
     stFirstPathParam.wFirstPathTrackNoiseFindWidth = 1000; // 跟踪径噪声搜索范围
 
@@ -87,14 +87,14 @@ void sysmain_init_lct()
     stLctCoordcalParam.dwMinimumValueNum = 5;
 
     // 判断二维基站第一径是否有效参数
-    stLctCoordcalParam.dwFirstPahtValueThres = 150;      // 二维第一径幅度有效门限
-    stLctCoordcalParam.dwFirstPahtSnrThres = 35;         // 二维第一径信噪有效门限(放大10倍)
-    stLctCoordcalParam.dwFirstPathIndexJumpThres = 1000; // 500; // 二维第一径跳变又有效门限
+    stLctCoordcalParam.dwFirstPahtValueThres = 150;     // 二维第一径幅度有效门限
+    stLctCoordcalParam.dwFirstPahtSnrThres = 35;        // 二维第一径信噪有效门限(放大10倍)
+    stLctCoordcalParam.dwFirstPathIndexJumpThres = 720; // 二维第一径跳变门限
 
     // 判断一维基站第一径是否有效参数
     stLctCoordcalParam.dwOneFirstPahtValueThres = 300;     // 一维第一径幅度有效门限
     stLctCoordcalParam.dwOneFirstPahtSnrThres = 50;        // 一维第一径信噪有效门限(放大10倍)
-    stLctCoordcalParam.dwOneFirstPathIndexJumpThres = 500; // 一维第一径跳变又有效门限
+    stLctCoordcalParam.dwOneFirstPathIndexJumpThres = 500; // 一维第一径跳变门限
 
     stLctCoordcalParam.dwBtsEdgeExpendLen = 150;      // 基站围成的矩形框向外扩展长度，默认值150
     stLctCoordcalParam.dwOneDimensionExpendLen = 200; // 一维定位时向外扩展长度，默认值200