完善代码：增加了利用toa选择极小值点的模块，但效果不如tdoa，暂时未用该方法。该方法是否可行，还需继续验证。

src/modules/location/lct_coordcal_one.c

@@ -116,7 +116,7 @@ void lct_coordcal_one_dimension_location(PUB_LOCATION_DATA_T *pstLctDataList, ui
                         wPoints = wRefPointMatrixRow * wRefPointMatrixCol; // 定位参考点数量
 
                         // 计算参考点时延误差
-                        lct_coordcal_cal_ref_point_delay(gLct_astLocationRefPoint, wRefPointMatrixRow, wRefPointMatrixCol, astValidBtsList, 2, byFirstPathFlag, (flt32 *)gLct_fDelayDeviationMatrix);
+                        lct_coordcal_cal_ref_point_delay_deviation_with_tdoa(gLct_astLocationRefPoint, wRefPointMatrixRow, wRefPointMatrixCol, astValidBtsList, 2, byFirstPathFlag, (flt32 *)gLct_fDelayDeviationMatrix);
 
                         // 选择线性参考点
                         lct_coordcal_sel_linear_ref_point(gLct_astLocationRefPoint, gLct_fDelayDeviationMatrix, wRefPointMatrixRow, wRefPointMatrixCol, &stLinearFunc, &stSelRefPoint, &fDelayDaviation);
@@ -176,7 +176,7 @@ void lct_coordcal_one_dimension_location(PUB_LOCATION_DATA_T *pstLctDataList, ui
                 wPoints = wRefPointMatrixRow * wRefPointMatrixCol; // 定位参考点数量
 
                 // 计算参考点时延误差
-                lct_coordcal_cal_ref_point_delay(gLct_astLocationRefPoint, wRefPointMatrixRow, wRefPointMatrixCol, astValidBtsList, 2, byFirstPathFlag, (flt32 *)gLct_fDelayDeviationMatrix);
+                lct_coordcal_cal_ref_point_delay_deviation_with_tdoa(gLct_astLocationRefPoint, wRefPointMatrixRow, wRefPointMatrixCol, astValidBtsList, 2, byFirstPathFlag, (flt32 *)gLct_fDelayDeviationMatrix);
 
                 // 选择线性参考点
                 lct_coordcal_sel_linear_ref_point(gLct_astLocationRefPoint, gLct_fDelayDeviationMatrix, wRefPointMatrixRow, wRefPointMatrixCol, &stLinearFunc, &stSelRefPoint, &fDelayDaviation);

src/modules/location/lct_coordcal_two.c

@@ -9,6 +9,7 @@
 extern COORD_T gLct_astLocationRefPoint[LCT_REF_POINT_MATRIX_LEN * LCT_REF_POINT_MATRIX_LEN]; // 定位参考点
 extern flt32 gLct_fDelayDeviationMatrix[LCT_REF_POINT_MATRIX_LEN * LCT_REF_POINT_MATRIX_LEN]; // 定位参考点时延误差
 extern LCT_COORDCAL_PARAM_T gg_stLctCoordcalParam;
+extern uint32 gLct_dwFpBaseOffset; // 第一径基础偏移量
 
 // 二维计算
 // pstLctBtsList:定位基站队列
@@ -234,10 +235,22 @@ void lct_coordcal_gen_location_ref_point(COORD_T stLeftBottom, COORD_T stRightTo
 // byFirstPathFlag：第一径类型标识(0:计算径，1：跟踪径)
 void lct_coordcal_cal_coord_points(LCT_FIRSTPATH_T *pstFirstPathList, uint8 byFirstPathListLen, COORD_T *pstLocationRefPoint, uint16 wMatrixRow, uint16 wMatrixCol, uint8 byFirstPathFlag, LCT_MINIMUM_VALUE_GROUP_T *pstMinmumValueGroup)
 {
-    // 计算定位参考点的时延误差
-    lct_coordcal_cal_ref_point_delay(pstLocationRefPoint, wMatrixRow, wMatrixCol, pstFirstPathList, byFirstPathListLen, byFirstPathFlag, (flt32 *)gLct_fDelayDeviationMatrix);
-
-    // 从各参考点“时延误差”中找出所有极小值点
+    // // 计算定位参考点的累计误差
+    // if (gLct_dwFpBaseOffset > 0)
+    // { // 已经计算出第一径基本偏移量
+    //     // 计算参考点距离累计误差
+    //     lct_coordcal_cal_ref_point_distance_deviation_with_toa(pstLocationRefPoint, wMatrixRow, wMatrixCol, pstFirstPathList, byFirstPathListLen, byFirstPathFlag, (flt32 *)gLct_fDelayDeviationMatrix);
+    // }
+    // else
+    // { // 尚未计算出第一径基本偏移量
+    //     // 计算参考点“时延累计误差”
+    //     lct_coordcal_cal_ref_point_delay_deviation_with_tdoa(pstLocationRefPoint, wMatrixRow, wMatrixCol, pstFirstPathList, byFirstPathListLen, byFirstPathFlag, (flt32 *)gLct_fDelayDeviationMatrix);
+    // }
+
+    // 计算定位参考点的时延累计误差
+    lct_coordcal_cal_ref_point_delay_deviation_with_tdoa(pstLocationRefPoint, wMatrixRow, wMatrixCol, pstFirstPathList, byFirstPathListLen, byFirstPathFlag, (flt32 *)gLct_fDelayDeviationMatrix);
+
+    // 从各参考点“误差”中找出所有极小值点
     lct_coordcal_sel_minimum_value((flt32 *)gLct_fDelayDeviationMatrix, wMatrixRow, wMatrixCol, pstLocationRefPoint, byFirstPathFlag, pstMinmumValueGroup);
 
     return;
@@ -252,7 +265,7 @@ void lct_coordcal_cal_coord_points(LCT_FIRSTPATH_T *pstFirstPathList, uint8 byFi
 // byFirstPathFlag:第一径标识（0：当前径，1：跟踪径）
 // pfDelayDeviationMatrix:各点时延误差矩阵（返回参数），行列值同输入行列参数
 //  返回值，1：成功，0：失败
-void lct_coordcal_cal_ref_point_delay(COORD_T *pstRefPointMatrix, uint16 wMatrixRow, uint16 wMatrixCol, LCT_FIRSTPATH_T *pstFirstPathList, uint8 byFirstPathListLen, uint8 byFirstPathFlag, flt32 *pfDelayDeviationMatrix)
+void lct_coordcal_cal_ref_point_delay_deviation_with_tdoa(COORD_T *pstRefPointMatrix, uint16 wMatrixRow, uint16 wMatrixCol, LCT_FIRSTPATH_T *pstFirstPathList, uint8 byFirstPathListLen, uint8 byFirstPathFlag, flt32 *pfDelayDeviationMatrix)
 {
     LCT_FIRSTPATH_T stMainBts; // 主站
     uint8 byExistFlag = 0;
@@ -367,6 +380,77 @@ void lct_coordcal_cal_ref_point_delay(COORD_T *pstRefPointMatrix, uint16 wMatrix
     return;
 }
 
+// 计算定位参考点与所有基站间的距离误差和
+// pstRefPointMatrix:参考点矩阵
+// wMatrixRow:参考点矩阵行
+// wMatrixCol:参考点矩阵列
+// pstFirstPathList:基站第一径列表,
+// byFirstPathListLen:基站第一径列表长度
+// byFirstPathFlag:第一径标识（0：当前径，1：跟踪径）
+// pfDelayDeviationMatrix:各点时延误差矩阵（返回参数），行列值同输入行列参数
+//  返回值，1：成功，0：失败
+void lct_coordcal_cal_ref_point_distance_deviation_with_toa(COORD_T *pstRefPointMatrix, uint16 wMatrixRow, uint16 wMatrixCol, LCT_FIRSTPATH_T *pstFirstPathList, uint8 byFirstPathListLen, uint8 byFirstPathFlag, flt32 *pfDelayDeviationMatrix)
+{
+    COORD_T stLctRefPointCoord; // 参考点坐标
+    uint16 wIndex = 0;
+
+    COORD_T stSlaveBtsCoord;    // 从站坐标
+    uint32 dwSlaveBtsIndex = 0; // 从站第一径索引
+    flt32 fSlaveSnr = 0;
+
+    flt32 fSlaveBtsDistance = 0;       // 参考点到从站的距离，单位厘米
+    flt32 fSlaveBtsSignalDistance = 0; // 从站信号距离
+
+    flt32 fDelayDeviation = 0;    // 时延误差
+    flt32 fDelayDeviationSum = 0; // 时延误差累计和
+
+    // 遍历参考点矩阵，计算各点时延误差累计和
+    for (uint16 y = 0; y < wMatrixRow; y++)
+    {
+        for (uint16 x = 0; x < wMatrixCol; x++)
+        {
+            wIndex = y * wMatrixCol + x;                    // 计算矩阵索引，
+            stLctRefPointCoord = pstRefPointMatrix[wIndex]; // 定位参考点坐标
+
+            // 组织从站的第一径索引数据和距离数据
+            for (uint8 i = 0; i < byFirstPathListLen; i++)
+            {
+                // 组织从站坐标
+                stSlaveBtsCoord.dwX = pstFirstPathList[i].dwCoordX;
+                stSlaveBtsCoord.dwY = pstFirstPathList[i].dwCoordY;
+                stSlaveBtsCoord.dwZ = pstFirstPathList[i].dwCoordZ;
+
+                // 组织从站第一径索引
+                if (byFirstPathFlag <= 0)
+                { // 计算径
+                    dwSlaveBtsIndex = pstFirstPathList[i].dwCalPathIndex;
+                    fSlaveSnr = pstFirstPathList[i].fCalPathIndexSnr;
+                }
+                else
+                { // 跟踪径
+                    dwSlaveBtsIndex = pstFirstPathList[i].dwTrackPathIndex;
+                    fSlaveSnr = pstFirstPathList[i].fTrackPathIndexSnr;
+                }
+
+                // 计算参考定位点到从站的距离
+                fSlaveBtsDistance = util_distance_cal_p2p_space_distance(stLctRefPointCoord.dwX, stLctRefPointCoord.dwY, stLctRefPointCoord.dwZ, stSlaveBtsCoord.dwX, stSlaveBtsCoord.dwY, stSlaveBtsCoord.dwZ);
+                lct_coordcal_cal_distance_by_toa(dwSlaveBtsIndex, &fSlaveBtsSignalDistance);
+
+                // 时延误差(单位毫秒)，即通过坐标计算的时延差与通过基站第一径索引计算的时延差之差
+                fDelayDeviation = fSlaveBtsDistance - fSlaveBtsSignalDistance;
+                // fDelayDeviationSum += fDelayDeviation * fDelayDeviation * fSlaveSnr;
+                fDelayDeviationSum += fDelayDeviation * fDelayDeviation;
+            }
+
+            // 保存参考点到各基站间的时延误差差和(单位毫秒)
+            pfDelayDeviationMatrix[wIndex] = fDelayDeviationSum;
+            fDelayDeviationSum = 0; // 时延误差清0
+        }
+    }
+
+    return;
+}
+
 // 计算两点间传输时长，单位：微妙
 // stFirstPoint:第1个点坐标，单位厘米
 // stSecondPoint:第2个点坐标，单位厘米
@@ -405,6 +489,18 @@ void lct_coordcal_cal_delay_by_first_path_index(uint32 dwMainBtsIndex, uint32 dw
     return;
 }
 
+// 通过第一径索引计算两个基站间的距离差
+// dwMainBtsIndex:主站第一径索引
+// dwSlaveBtsIndex:从站第一径索引
+// pfTwoBtsDistance:两基站间时延（单位米）
+void lct_coordcal_cal_distance_by_toa(uint32 dwBtsIndex, flt32 *pfTwoBtsDistance)
+{
+    int32 dwIndexGap = dwBtsIndex - gLct_dwFpBaseOffset;               // TOA值
+    *pfTwoBtsDistance = (dwIndexGap * PUB_AUDIO_SPEED) / (flt32)48000; // 米
+
+    return;
+}
+
 // 选择坐标
 // pfDelayDeviationSumMatrix:时延误差矩阵
 // wMatrixRow：时延误差矩阵行
@@ -653,6 +749,7 @@ void lct_coordcal_dbcheck_minimum_value(uint8 byFirstPathFlag, PUB_LCT_BTS_LIST_
 {
     uint8 byMinimumNum = 0;
     LCT_MINIMUM_VALUE_T *pstMiminumValueData = NULL;
+    uint32 dwCoordExpendLen = gg_stLctCoordcalParam.dwCoordExpendLen; // 定位基站围城的矩形框向外扩展长度
 
     int32 dwTemp = 0;
     int32 dwMinValueX = 2147483647;
@@ -742,7 +839,7 @@ void lct_coordcal_dbcheck_minimum_value(uint8 byFirstPathFlag, PUB_LCT_BTS_LIST_
         dwY = pstMiminumValueData->stCoord.dwY;
         if (dwGapY < 350)
         {
-            dwY = util_distance_gen_rand_num((dwMaxValueY + dwMinValueY) / 2, dwGapY / 10);
+            dwY = util_distance_gen_rand_num((dwMaxValueY + dwMinValueY) / 2, dwGapY / 2);
         }
         else if (dwGapY >= 350 && dwGapY < 900)
         {
@@ -754,11 +851,11 @@ void lct_coordcal_dbcheck_minimum_value(uint8 byFirstPathFlag, PUB_LCT_BTS_LIST_
         }
         pstMiminumValueData->stCoord.dwY = dwY;
 
-        // 再次检查坐标是否合法，极小值点要在基站围成的矩形框内部
-        dwMinValueX -= 300;
-        dwMaxValueX += 300;
-        dwMinValueY -= 300;
-        dwMaxValueY += 300;
+        // 再次检查坐标是否合法（极小值点要在基站围成的矩形框内部）
+        dwMinValueX -= dwCoordExpendLen;
+        dwMaxValueX += dwCoordExpendLen;
+        dwMinValueY -= dwCoordExpendLen;
+        dwMaxValueY += dwCoordExpendLen;
         if ((dwX >= dwMinValueX) && (dwX <= dwMaxValueX) && (dwY >= dwMinValueY) && (dwY <= dwMaxValueY))
         {
             if (byFirstPathFlag == CAL_PATH)

src/modules/location/lct_coordcal_two.h

@@ -16,10 +16,13 @@ void lct_coordcal_cal_rectangle_end_point_by_bts_coord(PUB_LCT_BTS_LIST_T *pstLc
 void lct_coordcal_cal_rectangle_two_end_point_by_coord(COORD_T stPredictionCoord, COORD_T *pstLeftBottom, COORD_T *pstRightTop);
 void lct_coordcal_gen_location_ref_point(COORD_T stLeftBottom, COORD_T stRightTop, uint8 byTrackFlag, COORD_T *pstLocationRefPoint, uint16 *pwMatrixRow, uint16 *pwMatrixCol);
 void lct_coordcal_cal_coord_points(LCT_FIRSTPATH_T *pstFirstPathList, uint8 byFirstPathListLen, COORD_T *pstLocationRefPoint, uint16 wMatrixRow, uint16 wMatrixCol, uint8 byFirstPathFlag, LCT_MINIMUM_VALUE_GROUP_T *pstMinmumValueGroup);
-void lct_coordcal_cal_ref_point_delay(COORD_T *pstRefPointMatrix, uint16 wMatrixRow, uint16 wMatrixCol, LCT_FIRSTPATH_T *pstFirstPathList, uint8 byFirstPathListLen, uint8 byFirstPathFlag, flt32 *pfDelayDeviationMatrix);
+void lct_coordcal_cal_ref_point_delay_deviation_with_tdoa(COORD_T *pstRefPointMatrix, uint16 wMatrixRow, uint16 wMatrixCol, LCT_FIRSTPATH_T *pstFirstPathList, uint8 byFirstPathListLen, uint8 byFirstPathFlag, flt32 *pfDelayDeviationMatrix);
 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, 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);
+
+void lct_coordcal_cal_ref_point_distance_deviation_with_toa(COORD_T *pstRefPointMatrix, uint16 wMatrixRow, uint16 wMatrixCol, LCT_FIRSTPATH_T *pstFirstPathList, uint8 byFirstPathListLen, uint8 byFirstPathFlag, flt32 *pfDelayDeviationMatrix);
+void lct_coordcal_cal_distance_by_toa(uint32 dwBtsIndex, flt32 *pfTwoBtsDistance);
 #endif

src/modules/location/lct_main.c

@@ -89,7 +89,7 @@ void lct_main(PUB_LCT_BTS_LIST_T *pstLctBtsList, PUB_LOCATION_DATA_T *pstLctData
     // 模块全局数据结构复位
     lct_main_reset();
 
-    // 系统已经同步并且定位基站数量至少有3个
+    // 系统已经同步并且定位基站数量至少有2个
     if (gg_stSysStatus.bySyncFlag && pstLctBtsList->byBtsNum > 1)
     {
         // 当前帧需要定位
@@ -163,7 +163,7 @@ 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_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);
@@ -203,6 +203,7 @@ void lct_main(PUB_LCT_BTS_LIST_T *pstLctBtsList, PUB_LOCATION_DATA_T *pstLctData
                     printf("minimum avalid\n");
                 }
             }
+            // 没有计算出极小值
             else
             {
                 pstCurrentLctResult->dwCpltFlag = 1;    // 定位完成

src/modules/location/lct_toa.c

@@ -131,4 +131,4 @@ uint32 lct_toa_cal_one_bts_first_path_value(COORD_T *pstBtsCoord, COORD_T *pstAh
     dwFirstPathIndex = (uint32)(fDistance * PUB_SIGNAL_SAMPLE_RATIO / PUB_AUDIO_SPEED);
 
     return dwFirstPathIndex;
-}
+}

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 300 // 极小值调整门限（300厘米）
-// #define LCT_COORDCAL_MINIMUM_VALUE_ADJUST_VALUE 50  // 极小值调整值（50厘米）
+#define LCT_COORDCAL_MINIMUM_VALUE_ADJUST_THRES 300 // 极小值调整门限（210厘米）
+// #define LCT_COORDCAL_MINIMUM_VALUE_ADJUST_VALUE 50  // 极小值调整值（35厘米）
 
 // 最优定位坐标选择模块
 #define LCT_COORDSEL_PP_TIME_GAP 1        // 两点之间的时长间隔（单位秒）

src/utils/distance.c

@@ -19,6 +19,21 @@ flt32 util_distance_cal_p2p_distance(int32 x1, int32 y1, int32 x2, int32 y2)
     return fDistance;
 }
 
+// 计算两点之间的距离
+// 输入参数，单位厘米
+// 返回参数，单位米
+flt32 util_distance_cal_p2p_space_distance(int32 x1, int32 y1, int32 z1, int32 x2, int32 y2, int32 z2)
+{
+    flt32 fDistance = 0.0;
+    flt32 xx = (flt32)(x1 - x2);
+    flt32 yy = (flt32)(y1 - y2);
+    flt32 zz = (flt32)(z1 - z2);
+
+    fDistance = (flt32)sqrt(xx * xx + yy * yy + zz * zz);
+    fDistance /= 100;
+    return fDistance;
+}
+
 // 根据一个数，在该数左右两侧范围内生成一个随机数
 // dwBaseData:基数
 // wRange:单边偏移

src/utils/distance.h

@@ -10,6 +10,7 @@
 
 // 供外部使用接口
 flt32 util_distance_cal_p2p_distance(int32 x1, int32 y1, int32 x2, int32 y2);
+flt32 util_distance_cal_p2p_space_distance(int32 x1, int32 y1, int32 z1, int32 x2, int32 y2, int32 z2);
 int32 util_distance_gen_rand_num(int32 dwBaseData, uint16 wRange);
 
 #endif