519 lines
15 KiB
C++
519 lines
15 KiB
C++
#include "Sub3Judge04Bgcd.h"
|
||
#include "HFactory.h"
|
||
|
||
Sub3Judge04Bgcd::Sub3Judge04Bgcd()
|
||
{
|
||
m_exam = __NEW__(TKM3Item);
|
||
}
|
||
|
||
Sub3Judge04Bgcd::~Sub3Judge04Bgcd()
|
||
{
|
||
__DELETE__(m_exam);
|
||
}
|
||
|
||
bool Sub3Judge04Bgcd::dealJudgeEnter()
|
||
{
|
||
if(m_exam->TestPro != ItemProFlagInit) return false;
|
||
if(!m_car->rtkEnabled() || !m_car->rtkEnabled(1)) return false;
|
||
|
||
const TChuanGan* cg = m_car->historyChuanGan();
|
||
const TRTKResult& RTKKM3 = cg->RTKKM3;
|
||
const std::string& ksdd = TableSysSet->get211();
|
||
|
||
if(RTKKM3.BaseLaneCount == 0) return false;
|
||
if(RTKKM3.BasePointInLaneNo == 0) return false;
|
||
|
||
m_itemv = TSub3Item04Bgcd();
|
||
m_car->setPUB_JDCC_ZT(0); //借道超车
|
||
|
||
if(ksdd != siteof::nj)
|
||
{
|
||
//ToDo:变更车道语音
|
||
}
|
||
//ToDo2:生成进项目事件
|
||
m_itemv.Temp_LJJL_1 = cg->ai_ljjl;
|
||
//1、变更车道项目距离
|
||
const std::vector<std::string>& s504 = TableSysSet->asArray504();
|
||
m_itemv.Temp_XMJL = s504.size() > 0 && s504[0] != "" ? std::atoi(s504[0].c_str()) : 150;
|
||
//2、是否不需要变道，只要判断距离就可以了
|
||
m_itemv.Temp_No_BGCD = s504.size() > 7 && s504[7] != "" ? std::atoi(s504[7].c_str()) : 0;
|
||
//3、不需要实际变道模式下，车辆离右边线距离变化阀值
|
||
m_itemv.Temp_No_BGCD_YuZhi = s504.size() > 8 && s504[8] != "" ? std::atoi(s504[8].c_str()) : 0;
|
||
//4、进入项目时，车身右侧中心点离右边线距离
|
||
if(m_itemv.Temp_No_BGCD == 1)
|
||
{
|
||
m_itemv.Temp_Right_JL = (RTKKM3.Body_RF_ToRightEdge + RTKKM3.Body_RB_ToRightEdge) / 2; //div
|
||
}
|
||
|
||
m_itemv.Temp_BasePointInLaneNo = RTKKM3.BasePointInLaneNo;
|
||
m_itemv.Temp_BaseLaneCount = RTKKM3.BaseLaneCount;
|
||
|
||
m_exam->TestPro = ItemProFlagJudge;
|
||
return true;
|
||
}
|
||
|
||
void Sub3Judge04Bgcd::dealJudgeItem()
|
||
{
|
||
HELP_COST_TIME("");
|
||
if(m_exam->TestPro != ItemProFlagJudge) return;
|
||
if(!m_car->rtkEnabled() || !m_car->rtkEnabled(1)) return;
|
||
|
||
const TChuanGan* cg = m_car->historyChuanGan();
|
||
const TRTKResult& RTKKM3 = cg->RTKKM3;
|
||
const TGpsInfo& gps = cg->real.gps;
|
||
const TSensorInfo& sor = cg->real.sensor;
|
||
const TChuanGan* his1 = m_car->historyChuanGan(1);
|
||
const TRTKResult& RTKKM3_1= his1->RTKKM3;
|
||
//const TGpsInfo& gps1 = his1->real.gps;
|
||
const TSensorInfo& sor1 = his1->real.sensor;
|
||
const std::string& ksdd = TableSysSet->get211();
|
||
|
||
if(RTKKM3.BaseLaneCount == 0) return;
|
||
if(RTKKM3.BasePointInLaneNo == 0) return;
|
||
|
||
if(ksdd == siteof::lyggy)
|
||
{
|
||
if(RTKKM3.BasePointInLaneNo == RTKKM3.BaseLaneCount)
|
||
{
|
||
m_itemv.Temp_Add_XMJL_Flag = true;
|
||
}
|
||
}
|
||
//如果路口项目没有结束，项目距离加100
|
||
if(ksdd == siteof::nj && m_exam->ItemNo == Sub3ItemType04Bgcd) //yhy???
|
||
{
|
||
if(m_car->isCrossroadExaming() && !m_itemv.Temp_Add_XMJL_Flag)
|
||
{
|
||
m_itemv.Temp_XMJL += 100;
|
||
m_itemv.Temp_Add_XMJL_Flag = true;
|
||
}
|
||
}
|
||
|
||
//特殊地方需求，不可以停车
|
||
if(ksdd == siteof::gdzjkm3)
|
||
{
|
||
if(m_car->historyMoveState(0) == moveStop &&
|
||
m_car->historyMoveState(1) == moveStop &&
|
||
m_car->historyMoveState(2) == moveStop &&
|
||
m_car->historyMoveState(3) == moveStop)
|
||
{
|
||
JUDGE_MARK_SUB3(4, "41", false);
|
||
}
|
||
}
|
||
|
||
//取消项目
|
||
//SysSet[378]变更车道和超车:取消第1个项目 (0-不能 1-不变道也不开转向灯 2-开双跳)
|
||
if(TableSysSet->get378() == "2" && sor.shtd == SYES)
|
||
{
|
||
if(m_exam->ItemEnterCancelID == false)
|
||
{
|
||
m_exam->TestPro = ItemProFlagIdle;
|
||
m_exam->Item_Color = itemStateWk;
|
||
m_exam->ItemEnterCancelID = true;
|
||
m_car->setPUB_JDCC_ZT(0); //借道超车
|
||
//ToDo:语音播报： 取消变更车道或者取消超车
|
||
m_car->createEventCancelItem({m_exam->ItemNo, ""});
|
||
return;
|
||
}
|
||
}
|
||
|
||
//左方向灯赋值
|
||
if(RTKKM3.TouchLineType != TRTKResult::TouchLineType0)
|
||
{
|
||
if(!m_itemv.JudgeFXD)
|
||
{
|
||
m_itemv.JudgeFXD = true;
|
||
if(sor.zfxd == SYES && sor.yfxd == SNOT)
|
||
{
|
||
m_itemv.Temp_ZFXD = true;
|
||
//跟踪3秒前
|
||
bool OKFlag = true;
|
||
for(int i = 1; i <= 14; i++)
|
||
{
|
||
const TSensorInfo& sori = m_car->historySensor(i);
|
||
if(sori.zfxd == SNOT || sori.yfxd == SYES)
|
||
{
|
||
OKFlag = false;
|
||
break;
|
||
}
|
||
}
|
||
m_itemv.Temp_ZFXD3s = OKFlag;
|
||
}
|
||
|
||
if(sor.zfxd == SNOT && sor.yfxd == SYES)
|
||
{
|
||
m_itemv.Temp_YFXD = true;
|
||
//跟踪3秒前
|
||
bool OKFlag = true;
|
||
for(int i = 1; i <= 14; i++)
|
||
{
|
||
const TSensorInfo& sori = m_car->historySensor(i);
|
||
if(sori.zfxd == SYES || sori.yfxd == SNOT)
|
||
{
|
||
OKFlag = false;
|
||
break;
|
||
}
|
||
}
|
||
m_itemv.Temp_YFXD3s = OKFlag;
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
|
||
/*
|
||
//左方向灯赋值
|
||
if(sor.zfxd == SYES && sor.yfxd == SNOT)
|
||
{
|
||
m_itemv.Temp_ZFXD = true;
|
||
if(m_itemv.Temp_ZFXD_TM == 0)
|
||
{
|
||
m_itemv.Temp_ZFXD_TM = gps.sj;
|
||
}
|
||
else
|
||
{
|
||
if(std::abs(gps.sj - m_itemv.Temp_ZFXD_TM) >= 3000)
|
||
{
|
||
m_itemv.Temp_ZFXD3s = true;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if(!m_itemv.Temp_ZFXD3s)
|
||
{
|
||
m_itemv.Temp_ZFXD_TM = 0;
|
||
}
|
||
}
|
||
|
||
//右方向灯赋值
|
||
if(sor.yfxd == SYES && sor.zfxd == SNOT)
|
||
{
|
||
m_itemv.Temp_YFXD = true;
|
||
if(m_itemv.Temp_YFXD_TM == 0)
|
||
{
|
||
m_itemv.Temp_YFXD_TM = gps.sj;
|
||
}
|
||
else
|
||
{
|
||
if(std::abs(gps.sj - m_itemv.Temp_YFXD_TM) >= 3000)
|
||
{
|
||
m_itemv.Temp_YFXD3s = true;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if(!m_itemv.Temp_YFXD3s)
|
||
{
|
||
m_itemv.Temp_YFXD_TM = 0;
|
||
}
|
||
}
|
||
*/
|
||
|
||
if(m_itemv.Temp_No_BGCD == 1)
|
||
{
|
||
int dis = (RTKKM3.Body_RF_ToRightEdge + RTKKM3.Body_RB_ToRightEdge) / 2; //div
|
||
if(int(std::abs(dis - m_itemv.Temp_Right_JL)) >= m_itemv.Temp_No_BGCD_YuZhi)
|
||
{
|
||
m_itemv.Temp_No_BGCD_FinishFlag = true;
|
||
}
|
||
}
|
||
|
||
//计算变道方向
|
||
int BianDaoKind = 0; //0:未变道 1：往左侧变道 2:往右侧变道
|
||
if(RTKKM3.BaseLaneCount == m_itemv.Temp_BaseLaneCount && RTKKM3.BasePointInLaneNo > m_itemv.Temp_BasePointInLaneNo)
|
||
{
|
||
BianDaoKind = 1;
|
||
}
|
||
//左侧变道2
|
||
if(RTKKM3.BaseLaneCount < m_itemv.Temp_BaseLaneCount)
|
||
{
|
||
int offset = m_itemv.Temp_BaseLaneCount - RTKKM3.BaseLaneCount;
|
||
if(offset == 1)
|
||
{
|
||
if(RTKKM3.BasePointInLaneNo <= m_itemv.Temp_BasePointInLaneNo)
|
||
{
|
||
BianDaoKind = 1;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
BianDaoKind = 1;
|
||
}
|
||
}
|
||
//左侧变道3
|
||
if(RTKKM3.BaseLaneCount > m_itemv.Temp_BaseLaneCount)
|
||
{
|
||
int offset = RTKKM3.BaseLaneCount - m_itemv.Temp_BaseLaneCount;
|
||
if(RTKKM3.BasePointInLaneNo >= m_itemv.Temp_BasePointInLaneNo + offset + 1)
|
||
{
|
||
BianDaoKind = 1;
|
||
}
|
||
}
|
||
|
||
//右侧变道1（因为我们测绘定义的缺陷，右侧变道暂不考虑车道数变多或者变少的情况)
|
||
if(RTKKM3.BaseLaneCount == m_itemv.Temp_BaseLaneCount && RTKKM3.BasePointInLaneNo < m_itemv.Temp_BasePointInLaneNo)
|
||
{
|
||
BianDaoKind = 2;
|
||
}
|
||
|
||
//特殊情况1:借道
|
||
//处理中心线虚线 左侧借道
|
||
if(RTKKM3.BaseLaneCount == 1)
|
||
{
|
||
if(RTKKM3.TouchLineType == TRTKResult::TouchLineType6 && RTKKM3_1.TouchLineType == TRTKResult::TouchLineType6)
|
||
{
|
||
if(RTKKM3.Wheel_LF_ToBaseLine < RTKKM3.Wheel_RF_ToBaseLine && RTKKM3_1.Wheel_LF_ToBaseLine < RTKKM3_1.Wheel_RF_ToBaseLine)
|
||
{
|
||
BianDaoKind = 1; //左侧借道
|
||
}
|
||
}
|
||
}
|
||
|
||
if(BianDaoKind == 1) //左侧变道
|
||
{
|
||
m_itemv.LeftBianDaoFlag = true;
|
||
}
|
||
else if(BianDaoKind == 2) //右侧变道
|
||
{
|
||
m_itemv.RightBianDaoFlag = true;
|
||
}
|
||
|
||
if(m_itemv.LeftBianDaoFlag == true || m_itemv.RightBianDaoFlag == true)
|
||
{
|
||
if(m_itemv.Status <= 2)
|
||
{
|
||
//SysSet[414]:变道、超车以前后轮都过线（0-否 1-是）
|
||
if(TableSysSet->get414() == "1")
|
||
{
|
||
if(RTKKM3.TouchLineType == TRTKResult::TouchLineType0)
|
||
{
|
||
m_itemv.Status = 3;
|
||
m_itemv.Temp_LJJL_2 = cg->ai_ljjl;
|
||
return;
|
||
}
|
||
else
|
||
{
|
||
m_itemv.Status = 13;
|
||
m_itemv.Temp_LJJL_2 = cg->ai_ljjl;
|
||
return;
|
||
}
|
||
|
||
}
|
||
else
|
||
{
|
||
if(ksdd != siteof::dingxi)
|
||
{
|
||
if(ksdd == siteof::chongqingkm3)
|
||
{
|
||
if(m_exam->ItemNo == Sub3ItemType04Bgcd)
|
||
{
|
||
if(sor.dw < 2 && sor1.dw < 2 && IS_C1(m_carType)) //20171210 20171216
|
||
{
|
||
m_itemv.Status = 4;
|
||
JUDGE_MARK_SUB3(4, "41", false); //20160928
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
m_itemv.Status = 3;
|
||
m_itemv.Temp_LJJL_2 = cg->ai_ljjl;
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if(cg->ai_ljjl - m_itemv.Temp_LJJL_1 >= m_itemv.Temp_XMJL) //出项目距离，结束
|
||
{
|
||
//到达这个距离，还没有完成项目，肯定要扣分的（除非你取消了项目）
|
||
//SysSet[378]:变更车道和超车:取消第1个项目 (0-不能 1-不变道也不开转向灯 2-开双跳)
|
||
if(TableSysSet->get378() == "1") //如果你正常完成了，不可能直行到项目距离这里
|
||
{
|
||
if(m_exam->ItemEnterCancelID == false)
|
||
{
|
||
m_exam->TestPro = ItemProFlagIdle;
|
||
m_exam->Item_Color = itemStateWk;
|
||
m_exam->ItemEnterCancelID = true;
|
||
m_car->setPUB_JDCC_ZT(0); //借道超车
|
||
//ToDo:播报语音：取消变更车道
|
||
m_car->createEventCancelItem({m_exam->ItemNo, ""});
|
||
return;
|
||
}
|
||
else
|
||
{
|
||
Judge_XMJL_Finish();
|
||
return;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
Judge_XMJL_Finish();
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
|
||
if(m_itemv.Status == 13)
|
||
{
|
||
if(RTKKM3.TouchLineType == TRTKResult::TouchLineType0)
|
||
{
|
||
//20181024
|
||
if(m_itemv.Temp_BasePointInLaneNo == RTKKM3.BasePointInLaneNo && m_itemv.Temp_BaseLaneCount == RTKKM3.BaseLaneCount)
|
||
{
|
||
//不按规定变道
|
||
//20180317
|
||
JUDGE_MARK_SUB3(4, "41", false); //20160928
|
||
m_itemv.Status = 4;
|
||
return;
|
||
}
|
||
else
|
||
{
|
||
m_itemv.Status = 3;
|
||
return;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
//20171207
|
||
if(ksdd == siteof::chongqingkm3)
|
||
{
|
||
if(sor.dw < 2 && sor1.dw < 2 && IS_C1(m_carType)) //20171210 20171216
|
||
{
|
||
m_itemv.Status = 4;
|
||
JUDGE_MARK_SUB3(4, "41", false); //20160928
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
if(m_itemv.Status == 3) //变道发生，延长一定距离后结束
|
||
{
|
||
|
||
if(cg->ai_ljjl - m_itemv.Temp_LJJL_2 >= 10) //延长距离，结束
|
||
{
|
||
if(TableSysSet->get414() == "1")
|
||
{
|
||
if(RTKKM3.TouchLineType == TRTKResult::TouchLineType0)
|
||
{
|
||
m_itemv.Status = 4;
|
||
return;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
m_itemv.Status = 4;
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
|
||
if(m_itemv.Status == 4)
|
||
{
|
||
m_exam->TestPro = ItemProFlagEnd;
|
||
flagEndJudge();
|
||
return;
|
||
}
|
||
|
||
char buf[128] = {0};
|
||
SafeSprintf(buf, sizeof(buf), XCharacter("Status=%d,车道数(初始,当前)-->%d,%d 车道号(初始,当前)-->%d,%d"),
|
||
m_itemv.Status, m_itemv.Temp_BaseLaneCount, RTKKM3.BaseLaneCount,
|
||
m_itemv.Temp_BasePointInLaneNo, RTKKM3.BasePointInLaneNo);
|
||
showStatus(std::string(buf));
|
||
}
|
||
|
||
void Sub3Judge04Bgcd::Judge_XMJL_Finish()
|
||
{
|
||
if(m_itemv.Temp_No_BGCD == 1)
|
||
{
|
||
if(!m_itemv.Temp_No_BGCD_FinishFlag)
|
||
{
|
||
JUDGE_MARK_SUB3(4, "41", false);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if(m_itemv.Status < 3)
|
||
{
|
||
JUDGE_MARK_SUB3(4, "41", false); //20160928
|
||
}
|
||
}
|
||
|
||
if(m_itemv.Temp_ZFXD == false && m_itemv.Temp_YFXD == false)
|
||
{
|
||
if(!m_itemv.Mark_4_42_Flag && m_itemv.Status >= 3) //小于3就是还没变道，不判未打灯
|
||
{
|
||
m_itemv.Mark_4_42_Flag = true;
|
||
JUDGE_MARK_SUB3(4, "42", false);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if(m_itemv.Temp_ZFXD3s == false && m_itemv.Temp_YFXD3s == false)
|
||
{
|
||
if(!m_itemv.Mark_4_43_Flag)
|
||
{
|
||
m_itemv.Mark_4_43_Flag = true;
|
||
JUDGE_MARK_SUB3(4, "43", false);
|
||
}
|
||
}
|
||
}
|
||
m_exam->TestPro = ItemProFlagEnd;
|
||
}
|
||
|
||
|
||
void Sub3Judge04Bgcd::flagEndJudge()
|
||
{
|
||
//finally:
|
||
if(m_exam->TestPro == ItemProFlagEnd)
|
||
{
|
||
//检查方向灯
|
||
if(m_itemv.LeftBianDaoFlag == true) //向左变道的
|
||
{
|
||
if(!m_itemv.Temp_ZFXD)
|
||
{
|
||
if(!m_itemv.Mark_4_42_Flag) // && m_itemv.Status >= 3 小于3就是还没变道，不判未打灯
|
||
{
|
||
m_itemv.Mark_4_42_Flag = true;
|
||
JUDGE_MARK_SUB3(4, "42", false);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if(!m_itemv.Temp_ZFXD3s)
|
||
{
|
||
if(!m_itemv.Mark_4_43_Flag)
|
||
{
|
||
m_itemv.Mark_4_43_Flag = true;
|
||
JUDGE_MARK_SUB3(4, "43", false);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
else if(m_itemv.RightBianDaoFlag == true) //向右变道的
|
||
{
|
||
if(!m_itemv.Temp_YFXD)
|
||
{
|
||
if(!m_itemv.Mark_4_42_Flag) // && m_itemv.Status >= 3 小于3就是还没变道，不判未打灯
|
||
{
|
||
m_itemv.Mark_4_42_Flag = true;
|
||
JUDGE_MARK_SUB3(4, "42", false);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if(!m_itemv.Temp_YFXD3s)
|
||
{
|
||
if(!m_itemv.Mark_4_43_Flag)
|
||
{
|
||
m_itemv.Mark_4_43_Flag = true;
|
||
JUDGE_MARK_SUB3(4, "43", false);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|