|
@@ -33,15 +33,8 @@ void lct_coordcal_one_dimension_location(PUB_LOCATION_DATA_T *pstLctDataList, ui
|
|
|
LCT_LINEAR_MINIMUM_LIST_T stLinearMinmumList = {0};
|
|
|
|
|
|
uint8 byCounter = 0;
|
|
|
- LCT_FIRSTPATH_T astValidBtsList[2] = {0}; // 1维基站对
|
|
|
- uint8 byValidBtsNum = 0; // 有效基站数量
|
|
|
- LCT_FIRSTPATH_T *pstMainBtsFirstPaht = NULL; // 主站
|
|
|
- LCT_FIRSTPATH_T *pstSlaveBtsFirstPaht = NULL; // 从站
|
|
|
-
|
|
|
- PUB_LOCATION_DATA_T *pstLctData = NULL;
|
|
|
- uint8 byFreqIndex = 0;
|
|
|
- int16 wIndexGap = 0;
|
|
|
- COORD_T stAheadCoord = {0}; // 上一个点坐标
|
|
|
+ LCT_FIRSTPATH_T astValidBtsList[2] = {0}; // 1维基站对
|
|
|
+ uint8 byValidBtsNum = 0; // 有效基站数量
|
|
|
|
|
|
uint16 wPoints = 0;
|
|
|
uint16 wRefPointMatrixRow = 0; // 参考点矩阵行
|
|
@@ -63,7 +56,8 @@ void lct_coordcal_one_dimension_location(PUB_LOCATION_DATA_T *pstLctDataList, ui
|
|
|
}
|
|
|
|
|
|
// 组织1维基站对(两个基站为1对)
|
|
|
- lct_coordcal_organize_one_dimension_bts_duad(byFirstPathFlag, pstValidBtsList, byValidBtsListLen, &stBtsDuadList);
|
|
|
+ // lct_coordcal_organize_one_dimension_bts_duad(byFirstPathFlag, pstValidBtsList, byValidBtsListLen, &stBtsDuadList);
|
|
|
+ lct_coordcal_one_organize_bts_duad(byFirstPathFlag, pstValidBtsList, byValidBtsListLen, &stBtsDuadList);
|
|
|
|
|
|
// 计算线性定位参考点
|
|
|
stLinearMinmumList.byValidFlag = 0;
|
|
@@ -81,7 +75,7 @@ void lct_coordcal_one_dimension_location(PUB_LOCATION_DATA_T *pstLctDataList, ui
|
|
|
// 基站2
|
|
|
stSecondCoord.dwX = astValidBtsList[1].dwCoordX;
|
|
|
stSecondCoord.dwY = astValidBtsList[1].dwCoordY;
|
|
|
- stFirstCoord.dwZ = astValidBtsList[1].dwCoordZ;
|
|
|
+ stSecondCoord.dwZ = astValidBtsList[1].dwCoordZ;
|
|
|
|
|
|
// 系统处于跟踪阶段
|
|
|
wPoints = 0;
|
|
@@ -90,7 +84,7 @@ void lct_coordcal_one_dimension_location(PUB_LOCATION_DATA_T *pstLctDataList, ui
|
|
|
// 两基站
|
|
|
if (stBtsDuadData.byBtsNum == 2)
|
|
|
{
|
|
|
- byTwoBtsLctFlag = 1; // 两站定位标识符
|
|
|
+ // byTwoBtsLctFlag = 1; // 两站定位标识符
|
|
|
|
|
|
// 检查预测坐标是否在一维基站内部
|
|
|
byInsideFlag = lct_coordcal_check_one_dimension_inside(stFirstCoord, stSecondCoord, stPredictionCoord);
|
|
@@ -111,6 +105,8 @@ void lct_coordcal_one_dimension_location(PUB_LOCATION_DATA_T *pstLctDataList, ui
|
|
|
// 两个基站有效,进行1维定位
|
|
|
if (byValidBtsNum == 2)
|
|
|
{
|
|
|
+ byTwoBtsLctFlag++;
|
|
|
+
|
|
|
// 生成线性参考点
|
|
|
lct_coordcal_build_linear_ref_point(byPredictionFlag, stPredictionCoord, stFirstCoord, stSecondCoord, &stLinearFunc, gLct_astLocationRefPoint, &wRefPointMatrixRow, &wRefPointMatrixCol);
|
|
|
wPoints = wRefPointMatrixRow * wRefPointMatrixCol; // 定位参考点数量
|
|
@@ -132,13 +128,6 @@ void lct_coordcal_one_dimension_location(PUB_LOCATION_DATA_T *pstLctDataList, ui
|
|
|
if (byResult)
|
|
|
{
|
|
|
wPoints = 1;
|
|
|
-
|
|
|
- // 二次校验定位坐标
|
|
|
-
|
|
|
- // // 调整无效基站的第一径
|
|
|
- // pstMainBtsFirstPaht = &astValidBtsList[0];
|
|
|
- // pstSlaveBtsFirstPaht = &pstValidBtsList[1];
|
|
|
- // lct_coordcal_adjust_first_path_index(pstLctDataList, byFirstPathFlag, &stSelRefPoint, pstMainBtsFirstPaht, pstSlaveBtsFirstPaht);
|
|
|
}
|
|
|
}
|
|
|
}
|
|
@@ -166,7 +155,7 @@ void lct_coordcal_one_dimension_location(PUB_LOCATION_DATA_T *pstLctDataList, ui
|
|
|
// 两级站定位
|
|
|
if (stBtsDuadData.byBtsNum == 2)
|
|
|
{
|
|
|
- byTwoBtsLctFlag = 1; // 两站定位标识符
|
|
|
+ byTwoBtsLctFlag++;
|
|
|
|
|
|
// 构建1维定位的线性方程
|
|
|
lct_coordcal_build_linear_func(stFirstCoord, stSecondCoord, &stLinearFunc);
|
|
@@ -234,12 +223,11 @@ void lct_coordcal_one_dimension_location(PUB_LOCATION_DATA_T *pstLctDataList, ui
|
|
|
// 校验定位坐标合法性
|
|
|
if (byFirstPathFlag == CAL_PATH)
|
|
|
{
|
|
|
- // 校验计算径结果
|
|
|
- lct_coordcal_check_linear_lct_coord_valid(1, &stLinearMinmumList);
|
|
|
-
|
|
|
// 保存输出参数
|
|
|
if (stLinearMinmumList.byCalMinimumListLen > 0)
|
|
|
{
|
|
|
+ lct_coordcal_check_linear_lct_coord_valid(1, &stLinearMinmumList); // 校验计算径结果
|
|
|
+
|
|
|
pstMinmumValueList->byLinearCalMinimumListLen = 1;
|
|
|
pstMinmumValueList->astLinearCalMinimumList[0].stCoord = stLinearMinmumList.astCalMinimumList[0].stCoord;
|
|
|
pstMinmumValueList->astLinearCalMinimumList[0].fDelayDeviationSum = stLinearMinmumList.astCalMinimumList[0].fDelayDeviationSum;
|
|
@@ -247,12 +235,11 @@ void lct_coordcal_one_dimension_location(PUB_LOCATION_DATA_T *pstLctDataList, ui
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- // 校验跟踪径结果
|
|
|
- lct_coordcal_check_linear_lct_coord_valid(2, &stLinearMinmumList);
|
|
|
-
|
|
|
// 保存输出参数
|
|
|
if (stLinearMinmumList.byTrackMinimumListLen > 0)
|
|
|
{
|
|
|
+ lct_coordcal_check_linear_lct_coord_valid(2, &stLinearMinmumList); // 校验跟踪径结果
|
|
|
+
|
|
|
pstMinmumValueList->byLinearTrackMinimumListLen = 1;
|
|
|
pstMinmumValueList->astLinearTrackMinimumList[0].stCoord = stLinearMinmumList.astTrackMinimumList[0].stCoord;
|
|
|
pstMinmumValueList->astLinearTrackMinimumList[0].fDelayDeviationSum = stLinearMinmumList.astTrackMinimumList[0].fDelayDeviationSum;
|
|
@@ -262,6 +249,114 @@ void lct_coordcal_one_dimension_location(PUB_LOCATION_DATA_T *pstLctDataList, ui
|
|
|
return;
|
|
|
}
|
|
|
|
|
|
+// 组织1维基站对(2个基站为1对)
|
|
|
+// byFirstPathType:第一径类型,0:计算径,1:跟踪径
|
|
|
+// pstValidBtsList:有效基站队列
|
|
|
+// byValidBtsListLen:有效基站队列长度
|
|
|
+// pstBtsDuadList:基站对队列
|
|
|
+void lct_coordcal_one_organize_bts_duad(uint8 byFirstPathType, LCT_FIRSTPATH_T *pstValidBtsList, uint8 byValidBtsListLen, LCT_COORDCAL_ONE_DIMENSION_BTS_DUAD_LIST_T *pstBtsDuadList)
|
|
|
+{
|
|
|
+ uint8 byBtsDuadCounter = 0; // 计算对计数器
|
|
|
+ LCT_COORDCAL_ONE_DIMENSION_BTS_DUAD_T stBtsDuadData = {0};
|
|
|
+
|
|
|
+ LCT_FIRSTPATH_T *pstMainBtsPath = NULL;
|
|
|
+ uint32 dwMainBtsId = 0;
|
|
|
+
|
|
|
+ LCT_FIRSTPATH_T *pstSlaveBtsPath = NULL;
|
|
|
+ uint32 dwSlaveBtsId = 0;
|
|
|
+
|
|
|
+ flt32 fSnr = 0;
|
|
|
+ flt32 fMaxValue = 0;
|
|
|
+
|
|
|
+ // 组织1维基站对(两个基站为1对)
|
|
|
+ for (uint8 i = 0; i < byValidBtsListLen; i++)
|
|
|
+ {
|
|
|
+ pstMainBtsPath = &pstValidBtsList[i]; // 主站第一径数
|
|
|
+ dwMainBtsId = pstMainBtsPath->dwBtsId; // 主站ID
|
|
|
+
|
|
|
+ // 主站定位类型,1:1维定位,2:二维定位,3:1维和2维定位
|
|
|
+ fSnr = 0;
|
|
|
+ fMaxValue = 0;
|
|
|
+ if (pstMainBtsPath->dwLctType == 1 || pstMainBtsPath->dwLctType == 3)
|
|
|
+ {
|
|
|
+ fMaxValue = pstMainBtsPath->fMaxAmplValue; // 最大值
|
|
|
+
|
|
|
+ // 保存基站snr
|
|
|
+ if (byFirstPathType == CAL_PATH)
|
|
|
+ {
|
|
|
+ fSnr = pstMainBtsPath->fCalPathIndexSnr;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ fSnr = pstMainBtsPath->fTrackPathIndexSnr;
|
|
|
+ }
|
|
|
+
|
|
|
+ // 遍历要与主站进行两站定位的从站
|
|
|
+ for (uint8 j = 0; j < pstMainBtsPath->dwTwoBtsNum; j++)
|
|
|
+ {
|
|
|
+ dwSlaveBtsId = pstMainBtsPath->adwTwoBtsList[j]; // 从站ID
|
|
|
+ for (uint8 k = i + 1; k < byValidBtsListLen; k++)
|
|
|
+ {
|
|
|
+ pstSlaveBtsPath = &pstValidBtsList[k]; // 从站第一径数据
|
|
|
+ if (pstSlaveBtsPath->dwBtsId == dwSlaveBtsId)
|
|
|
+ {
|
|
|
+ pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].dwFirstBtsId = dwMainBtsId;
|
|
|
+ pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].dwSecondBtsId = dwSlaveBtsId;
|
|
|
+ pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].byBtsNum = 2;
|
|
|
+ memcpy(&pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[0], pstMainBtsPath, sizeof(LCT_FIRSTPATH_T));
|
|
|
+ memcpy(&pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[1], pstSlaveBtsPath, sizeof(LCT_FIRSTPATH_T));
|
|
|
+
|
|
|
+ // 修改主站标识符
|
|
|
+ pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[0].byMainFlag = 0;
|
|
|
+ pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[1].byMainFlag = 0;
|
|
|
+ if (pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[0].fCalPathIndexSnr > pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[1].fCalPathIndexSnr)
|
|
|
+ {
|
|
|
+ pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[0].byMainFlag = 1;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].astBtsList[1].byMainFlag = 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ // 保存基站snr
|
|
|
+ if (byFirstPathType == CAL_PATH)
|
|
|
+ {
|
|
|
+ fSnr += pstSlaveBtsPath->fCalPathIndexSnr;
|
|
|
+ fMaxValue += pstSlaveBtsPath->fMaxAmplValue;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ fSnr += pstSlaveBtsPath->fTrackPathIndexSnr;
|
|
|
+ fMaxValue += pstSlaveBtsPath->fMaxAmplValue;
|
|
|
+ }
|
|
|
+ pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].fSnr = fSnr;
|
|
|
+ pstBtsDuadList->astBtsDuadList[byBtsDuadCounter].fAmplMaxValue = fMaxValue;
|
|
|
+
|
|
|
+ pstBtsDuadList->byBtsDuadNum++;
|
|
|
+ byBtsDuadCounter++;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ // 对最大值进行排序
|
|
|
+ for (uint8 i = 0; i < pstBtsDuadList->byBtsDuadNum; i++)
|
|
|
+ {
|
|
|
+ for (uint8 j = i + 1; j < pstBtsDuadList->byBtsDuadNum; j++)
|
|
|
+ {
|
|
|
+ if (pstBtsDuadList->astBtsDuadList[j].fAmplMaxValue > pstBtsDuadList->astBtsDuadList[i].fAmplMaxValue)
|
|
|
+ {
|
|
|
+ stBtsDuadData = pstBtsDuadList->astBtsDuadList[i];
|
|
|
+ pstBtsDuadList->astBtsDuadList[i] = pstBtsDuadList->astBtsDuadList[j];
|
|
|
+ pstBtsDuadList->astBtsDuadList[j] = stBtsDuadData;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return;
|
|
|
+}
|
|
|
+
|
|
|
// 组织1维基站对(2个基站为1对)
|
|
|
// byFirstPathType:第一径类型,0:计算径,1:跟踪径
|
|
|
// pstValidBtsList:有效基站队列
|
|
@@ -477,11 +572,6 @@ void lct_coordcal_check_one_dimension_bts_firstpath_valid(uint8 byPredictionFlag
|
|
|
uint16 wIndexGap = 0;
|
|
|
uint8 byCounter = 0;
|
|
|
|
|
|
- COORD_T stFirstBts = {0}; // 第一个基站坐标
|
|
|
- flt32 fDistFirstBts = 0;
|
|
|
- COORD_T stSecondBts = {0}; // 第二个基站坐标
|
|
|
- flt32 fDistSecondBts = 0;
|
|
|
-
|
|
|
// 检查基站有效性
|
|
|
for (uint8 i = 0; i < 2; i++)
|
|
|
{
|
|
@@ -677,8 +767,6 @@ void lct_coordcal_build_linear_func(COORD_T stBts1Coord, COORD_T stBts2Coord, LC
|
|
|
// pstCoord:坐标
|
|
|
void lct_coordcal_recal_linear_ref_point(LCT_COORDCAL_LINEAR_FUNC_T *pstLinearFunc, COORD_T *pstCoord)
|
|
|
{
|
|
|
- int32 dwTmpCoord = 0;
|
|
|
-
|
|
|
//// 二次修正极小值
|
|
|
// 不垂直于坐标轴
|
|
|
if (pstLinearFunc->byVerticalFlag == 0)
|
|
@@ -770,13 +858,10 @@ void lct_coordcal_check_linear_lct_coord_valid(uint8 byMinmumValueType, LCT_LINE
|
|
|
|
|
|
LCT_LINEAR_LCT_COORD_T astLinearLctCoord[LCT_MINIMUM_NUM] = {0};
|
|
|
uint8 byCounter = 0;
|
|
|
- COORD_T stCoord = {0};
|
|
|
flt32 fDeviation = 0;
|
|
|
- flt32 fMinDeviation = FLT_MAX;
|
|
|
uint8 byMinIndex = 0;
|
|
|
flt32 fX = 0;
|
|
|
flt32 fY = 0;
|
|
|
- flt32 fZ = 0;
|
|
|
|
|
|
// 极小值长度
|
|
|
if (byMinmumValueType == 1)
|
|
@@ -1099,7 +1184,7 @@ uint8 lct_coordcal_zero_dimension_location(PUB_LOCATION_DATA_T *pstLctDataList,
|
|
|
flt32 fOffsetDistanceX = 0; // X轴偏移距离
|
|
|
flt32 fOffsetDistanceY = 0; // X轴偏移距离
|
|
|
flt32 fFactor = 0;
|
|
|
- uint8 byResult = 0;
|
|
|
+ uint8 byResult = 0; // 返回结果
|
|
|
|
|
|
// 平滑坐标队列中有数据
|
|
|
if (gLct_stMeanLctCoordList.byListLen > 0)
|
|
@@ -1142,18 +1227,20 @@ uint8 lct_coordcal_zero_dimension_location(PUB_LOCATION_DATA_T *pstLctDataList,
|
|
|
// 轴速度不为0
|
|
|
if (gLct_dwSpeedY != 0 && gLct_dwSpeedX != 0)
|
|
|
{
|
|
|
- fFactor = abs(gLct_dwSpeedY) / abs(gLct_dwSpeedX);
|
|
|
- fOffsetDistance = fOffsetDistance * fOffsetDistance;
|
|
|
- fOffsetDistance /= fFactor * fFactor + 1;
|
|
|
+ fFactor = (flt32)(abs(gLct_dwSpeedY)) / abs(gLct_dwSpeedX);
|
|
|
+ // fOffsetDistance = fOffsetDistance * fOffsetDistance;
|
|
|
+ // fOffsetDistance /= fFactor * fFactor + 1;
|
|
|
|
|
|
- fOffsetDistanceX = fOffsetDistance;
|
|
|
fOffsetDistanceY = fOffsetDistance * fFactor;
|
|
|
+ fOffsetDistanceX = fOffsetDistance * (1 - fFactor);
|
|
|
}
|
|
|
+ // X轴速度为0
|
|
|
else if (gLct_dwSpeedX == 0)
|
|
|
{
|
|
|
fOffsetDistanceX = 0;
|
|
|
fOffsetDistanceY = fOffsetDistance;
|
|
|
}
|
|
|
+ // Y轴速度为0
|
|
|
else if (gLct_dwSpeedY == 0)
|
|
|
{
|
|
|
fOffsetDistanceX = fOffsetDistance;
|
|
@@ -1167,27 +1254,43 @@ uint8 lct_coordcal_zero_dimension_location(PUB_LOCATION_DATA_T *pstLctDataList,
|
|
|
// X轴轴距大
|
|
|
if (pstLinearFunc->byDirection == 1)
|
|
|
{
|
|
|
- stAheadCoord.dwX += gLct_byDirectionX * fOffsetDistanceX; // 移动距离是Y轴的3倍
|
|
|
- stAheadCoord.dwY = pstLinearFunc->fK * stAheadCoord.dwX + pstLinearFunc->fB;
|
|
|
+ if (gLct_byDirectionX != 0)
|
|
|
+ {
|
|
|
+ stAheadCoord.dwX += gLct_byDirectionX * fOffsetDistanceX; // 移动距离是Y轴的3倍
|
|
|
+ stAheadCoord.dwY = pstLinearFunc->fK * stAheadCoord.dwX + pstLinearFunc->fB;
|
|
|
+ byResult = 1;
|
|
|
+ }
|
|
|
}
|
|
|
// Y轴轴距大
|
|
|
else
|
|
|
{
|
|
|
- stAheadCoord.dwY += gLct_byDirectionY * fOffsetDistanceY; // 移动距离是X轴的3倍
|
|
|
- stAheadCoord.dwX = (int32)(stAheadCoord.dwY - pstLinearFunc->fB) / pstLinearFunc->fK;
|
|
|
+ if (gLct_byDirectionY != 0)
|
|
|
+ {
|
|
|
+ stAheadCoord.dwY += gLct_byDirectionY * fOffsetDistanceY; // 移动距离是X轴的3倍
|
|
|
+ stAheadCoord.dwX = (int32)(stAheadCoord.dwY - pstLinearFunc->fB) / pstLinearFunc->fK;
|
|
|
+ byResult = 1;
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
// 垂直于X坐标轴
|
|
|
else if (pstLinearFunc->byVerticalFlag == 1)
|
|
|
{
|
|
|
- stAheadCoord.dwX = pstLinearFunc->dwValueCoordX;
|
|
|
- stAheadCoord.dwY += gLct_byDirectionY * fOffsetDistanceY;
|
|
|
+ if (gLct_byDirectionY != 0)
|
|
|
+ {
|
|
|
+ stAheadCoord.dwX = pstLinearFunc->dwValueCoordX;
|
|
|
+ stAheadCoord.dwY += gLct_byDirectionY * fOffsetDistanceY;
|
|
|
+ byResult = 1;
|
|
|
+ }
|
|
|
}
|
|
|
// 垂直于Y坐标轴
|
|
|
else if (pstLinearFunc->byVerticalFlag == 2)
|
|
|
{
|
|
|
- stAheadCoord.dwX += gLct_byDirectionX * fOffsetDistanceX;
|
|
|
- stAheadCoord.dwY = pstLinearFunc->dwValueCoordY;
|
|
|
+ if (gLct_byDirectionX != 0)
|
|
|
+ {
|
|
|
+ stAheadCoord.dwX += gLct_byDirectionX * fOffsetDistanceX;
|
|
|
+ stAheadCoord.dwY = pstLinearFunc->dwValueCoordY;
|
|
|
+ byResult = 1;
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
// 第一径偏移量无效
|
|
@@ -1198,20 +1301,22 @@ uint8 lct_coordcal_zero_dimension_location(PUB_LOCATION_DATA_T *pstLctDataList,
|
|
|
{
|
|
|
stAheadCoord.dwX += gLct_dwSpeedX;
|
|
|
stAheadCoord.dwY += gLct_dwSpeedY;
|
|
|
+ byResult = 1;
|
|
|
}
|
|
|
else if (gLct_byDirectionX != 0)
|
|
|
{
|
|
|
stAheadCoord.dwX += gLct_dwSpeedX;
|
|
|
+ byResult = 1;
|
|
|
}
|
|
|
else if (gLct_byDirectionY != 0)
|
|
|
{
|
|
|
stAheadCoord.dwY += gLct_dwSpeedY;
|
|
|
+ byResult = 1;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
// 保存数据
|
|
|
memcpy(pstRefPoint, &stAheadCoord, sizeof(COORD_T));
|
|
|
- byResult = 1;
|
|
|
}
|
|
|
|
|
|
return byResult;
|
|
@@ -1233,8 +1338,8 @@ void lct_coordcal_adjust_first_path_index(PUB_LOCATION_DATA_T *pstLctDataList, u
|
|
|
fSlaveBtsDist = util_distance_cal_p2p_distance(pstLctCoord->dwX, pstLctCoord->dwY, pstSlaveBtsFirstPath->dwCoordX, pstSlaveBtsFirstPath->dwCoordY);
|
|
|
|
|
|
// 基站从站帧内索引
|
|
|
- fTimeDelay = ((fMainBtsDist - fSlaveBtsDist) / 343) * 1000 * 1000; // 单位微妙
|
|
|
- dwIndexDiff = (fTimeDelay * 48) / 1000; // 相位差
|
|
|
+ fTimeDelay = ((fMainBtsDist - fSlaveBtsDist) / PUB_AUDIO_SPEED) * 1000 * 1000; // 单位微妙
|
|
|
+ dwIndexDiff = (fTimeDelay * 48) / 1000; // 相位差
|
|
|
|
|
|
// 修正从站第一径索引
|
|
|
byFreqIndex = pstSlaveBtsFirstPath->byFreqIndex;
|
|
@@ -1267,7 +1372,7 @@ uint8 lct_coordcal_single_bts_location(PUB_LOCATION_DATA_T *pstLctDataList, LCT_
|
|
|
flt32 fOffsetDistance = 0; // 前后两点偏移距离
|
|
|
uint8 byMoveDirection = 0; // 1:X轴,2:Y轴
|
|
|
COORD_T stAheadCoord = {0};
|
|
|
- uint8 byResult = 0;
|
|
|
+ uint8 byResult = 0; // 返回结果
|
|
|
|
|
|
// 平滑坐标队列中至少有2个点
|
|
|
if (gLct_stMeanLctCoordList.byListLen > 1)
|
|
@@ -1305,12 +1410,13 @@ uint8 lct_coordcal_single_bts_location(PUB_LOCATION_DATA_T *pstLctDataList, LCT_
|
|
|
if (byValidFlag)
|
|
|
{
|
|
|
fOffsetDistance = abs(wIndexGap) * 0.7; // 偏移距离,可正也可负
|
|
|
- if (fOffsetDistance > 250 || fOffsetDistance < 150)
|
|
|
+ if (fOffsetDistance > 250 || fOffsetDistance < 120)
|
|
|
{
|
|
|
- fOffsetDistance = 200;
|
|
|
+ fOffsetDistance = 185;
|
|
|
}
|
|
|
|
|
|
- // 计算定位点坐标
|
|
|
+ //// 计算定位点坐标
|
|
|
+ // 在X轴与Y轴上都有移动
|
|
|
if (gLct_byDirectionX != 0 && gLct_byDirectionY != 0)
|
|
|
{
|
|
|
fOffsetDistance = fOffsetDistance * fOffsetDistance;
|
|
@@ -1329,14 +1435,17 @@ uint8 lct_coordcal_single_bts_location(PUB_LOCATION_DATA_T *pstLctDataList, LCT_
|
|
|
stAheadCoord.dwX += gLct_byDirectionX * fOffsetDistance;
|
|
|
stAheadCoord.dwY += gLct_byDirectionY * fOffsetDistance * 3; // 移动距离是X轴的3倍
|
|
|
}
|
|
|
+ byResult = 1;
|
|
|
}
|
|
|
else if (gLct_byDirectionX != 0)
|
|
|
{
|
|
|
stAheadCoord.dwX += gLct_byDirectionX * fOffsetDistance;
|
|
|
+ byResult = 1;
|
|
|
}
|
|
|
else if (gLct_byDirectionY != 0)
|
|
|
{
|
|
|
stAheadCoord.dwY += gLct_byDirectionY * fOffsetDistance;
|
|
|
+ byResult = 1;
|
|
|
}
|
|
|
}
|
|
|
// 第一径偏移量无效
|
|
@@ -1347,20 +1456,22 @@ uint8 lct_coordcal_single_bts_location(PUB_LOCATION_DATA_T *pstLctDataList, LCT_
|
|
|
{
|
|
|
stAheadCoord.dwX += gLct_dwSpeedX;
|
|
|
stAheadCoord.dwY += gLct_dwSpeedY;
|
|
|
+ byResult = 1;
|
|
|
}
|
|
|
else if (gLct_byDirectionX != 0)
|
|
|
{
|
|
|
stAheadCoord.dwX += gLct_dwSpeedX;
|
|
|
+ byResult = 1;
|
|
|
}
|
|
|
else if (gLct_byDirectionY != 0)
|
|
|
{
|
|
|
stAheadCoord.dwY += gLct_dwSpeedY;
|
|
|
+ byResult = 1;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
// 保存数据
|
|
|
memcpy(pstRefPoint, &stAheadCoord, sizeof(COORD_T));
|
|
|
- byResult = 1;
|
|
|
}
|
|
|
|
|
|
return byResult;
|