You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
276 lines
10 KiB
C++
276 lines
10 KiB
C++
#include "collisiondetectionthread.h"
|
|
#include "qdebug.h"
|
|
#include <QPointF>
|
|
|
|
//#pragma execution_character_set("utf-8")
|
|
|
|
CollisionDetectionThread::CollisionDetectionThread(QMapControl* pMapControl,QObject *parent)
|
|
: QObject{parent}
|
|
{
|
|
// trayIcon = new QSystemTrayIcon(this);
|
|
|
|
this->detectFlag = false;
|
|
this->endFlag = false;
|
|
this->pMapControl = pMapControl;
|
|
this->ms = 1000;
|
|
|
|
this->collisionX = -1;
|
|
this->collisionY = -1;
|
|
|
|
resetWarning();
|
|
}
|
|
|
|
CollisionDetectionThread::~CollisionDetectionThread()
|
|
{
|
|
// delete trayIcon;
|
|
|
|
}
|
|
|
|
//执行地形碰撞检测子线程逻辑
|
|
void CollisionDetectionThread::working(double lon,double lat,double relativeAlititude,double heading,double speed)
|
|
{
|
|
double detectDist = speed * 60;//计算探测距离
|
|
int offsetDist = 10; //偏移距离，考虑到左右机翼长度
|
|
//计算当前坐标点垂直左右偏点
|
|
QPointF curLPointOffset = geoComputation.getDestinationPoint(lon,lat,heading-90,offsetDist);
|
|
QPointF curRPointOffset = geoComputation.getDestinationPoint(lon,lat,heading+90,offsetDist);
|
|
|
|
QList<QPointF> ptList;
|
|
ptList.append(curLPointOffset);
|
|
ptList.append(QPointF(lon,lat));
|
|
ptList.append(curRPointOffset);
|
|
|
|
double collisionDistance = -1;
|
|
QPointF collisionPt;
|
|
double collisionAlititude;
|
|
foreach(QPointF pt,ptList)
|
|
{
|
|
QPointF destPt = geoComputation.getDestinationPoint(pt.x(),pt.y(),heading,detectDist);
|
|
UGPoint2D pntView(pt.x(),pt.y());//观察点
|
|
UGPoint2D pntObject(destPt.x(),destPt.y()); //目标点
|
|
UG3DAnalyst::SingleResult* result = spatialAnalysis.DoublePointVisibility(pMapControl,pntView,pntObject,relativeAlititude);
|
|
if(!result->bVisible)
|
|
{
|
|
//计算碰撞距离
|
|
double tmpDist = geoComputation.getSphericalDistance(pt.x(),pt.y(),result->pnt3D.x,result->pnt3D.y);
|
|
if(collisionDistance<0)
|
|
{
|
|
collisionDistance = tmpDist;
|
|
collisionPt.setX(result->pnt3D.x);
|
|
collisionPt.setY(result->pnt3D.y);
|
|
collisionAlititude = result->pnt3D.z;
|
|
}
|
|
else if(tmpDist<collisionDistance)
|
|
{//获取距离最近的一个碰撞点
|
|
collisionDistance = tmpDist;
|
|
collisionPt.setX(result->pnt3D.x);
|
|
collisionPt.setY(result->pnt3D.y);
|
|
collisionAlititude = result->pnt3D.z;
|
|
}
|
|
}
|
|
};
|
|
|
|
if(collisionDistance<0) return;
|
|
else
|
|
{
|
|
double collisionLon = collisionPt.x();
|
|
double collisionLat = collisionPt.y();
|
|
// qDebug()<<"*******curLon:"<<QString::number(lon,'f',6)<<" curLat:"<<QString::number(lat,'f',6)<<" collisionLon:"<<QString::number(collisionLon,'f',6)<<" collisionLat:"<<QString::number(collisionLat,'f',6);
|
|
//判断是否同一碰撞点，不是的话进行预警重置
|
|
if(!isIdenticalCollisionPoint(collisionLon,collisionLat)) resetWarning();
|
|
//预警逻辑实现
|
|
int collisionTime = collisionDistance/speed;
|
|
/***********************预警提示**************************/
|
|
showWarningNotify(collisionTime,collisionDistance,collisionAlititude);
|
|
}
|
|
|
|
/*
|
|
//计算目标点
|
|
QPointF destPoint = geoComputation.getDestinationPoint(lon,lat,heading,detectDist);
|
|
// QPointF destLPointOffset = geoComputation.getDestinationPoint(curLPointOffset.x(),curLPointOffset.y(),heading,detectDist);
|
|
// QPointF destRPointOffset = geoComputation.getDestinationPoint(curRPointOffset.x(),curRPointOffset.x(),heading,detectDist);
|
|
// qDebug()<<"*******lon:"<<QString::number(destPoint.x(),'f',6)<<" lat:"<<QString::number(destPoint.y(),'f',6);
|
|
//进行通视分析
|
|
UGPoint2D pntView(lon,lat);//观察点
|
|
UGPoint2D pntObject(destPoint.x(),destPoint.y()); //目标点
|
|
UG3DAnalyst::SingleResult* result = spatialAnalysis.DoublePointVisibility(pMapControl,pntView,pntObject,relativeAlititude);
|
|
if(result->bVisible)
|
|
{//两点通视，无障碍，不做任何处理
|
|
return;
|
|
}
|
|
else
|
|
{//发现碰撞点，进行预警提示
|
|
//第一碰撞点
|
|
double collisionLon = result->pnt3D.x;
|
|
double collisionLat = result->pnt3D.y;
|
|
qDebug()<<"*******curLon:"<<QString::number(lon,'f',6)<<" curLat:"<<QString::number(lat,'f',6)<<" collisionLon:"<<QString::number(collisionLon,'f',6)<<" collisionLat:"<<QString::number(collisionLat,'f',6);
|
|
//判断是否同一碰撞点，不是的话进行预警重置
|
|
if(!isIdenticalCollisionPoint(collisionLon,collisionLat)) resetWarning();
|
|
|
|
//计算碰撞距离(m)
|
|
double collisionDistance = geoComputation.getSphericalDistance(lon,lat,collisionLon,collisionLat);
|
|
//预警逻辑实现
|
|
int collisionTime = collisionDistance/speed;
|
|
//预警提示
|
|
showWarningNotify(collisionTime,collisionDistance,result->pnt3D.z);
|
|
}
|
|
*/
|
|
// this->detectFlag = false;
|
|
}
|
|
|
|
void CollisionDetectionThread::start(int ms)
|
|
{
|
|
emit requestPositionInfo(ms);
|
|
}
|
|
|
|
//接收飞机位置信息
|
|
void CollisionDetectionThread::receiveLocationData(QList<double> lon,QList<double> lat,QList<double> relativeAlititude,
|
|
QList<double> heading,QList<double> speed)
|
|
{
|
|
// this->currentLat1 = currentLat1;
|
|
// this->currentLon1 = currentLon1;
|
|
// this->heading = heading;
|
|
// this->relativeAltitude = relativeAltitude;
|
|
// this->speed = speed;
|
|
// qDebug()<<"*********************Lon:"<<lon.takeLast();
|
|
if(lon.isEmpty()) return;
|
|
for(int i=0;i<lon.size();i++)
|
|
{
|
|
working(lon.at(i),lat.at(i),relativeAlititude.at(i),heading.at(i),speed.at(i));
|
|
}
|
|
// this->detectFlag = true;
|
|
}
|
|
|
|
//关闭线程
|
|
void CollisionDetectionThread::closedCollisionDetection()
|
|
{
|
|
emit stopSendData();//通知RealTimePositionInfoThread停止发送数据
|
|
}
|
|
|
|
//判断是否同一碰撞点
|
|
bool CollisionDetectionThread::isIdenticalCollisionPoint(double collisionLon,double collisionLat)
|
|
{
|
|
UGPoint2D pt(collisionLon,collisionLat);
|
|
UGDatasetRasterPtr datasetRaster = pMapControl->getDemDataSet(pt);
|
|
UGPoint pntImg;
|
|
datasetRaster->XYToImg(pt,pntImg);
|
|
datasetRaster = NULL;
|
|
// qDebug()<<"**************x:"<<pntImg.x<<" y:"<<pntImg.y;
|
|
// qDebug()<<"*****collisionX:"<<this->collisionX<<" y:"<<this->collisionY;
|
|
// if(this->collisionX == pntImg.x && this->collisionY==pntImg.y)
|
|
// {//同一碰撞点
|
|
// qDebug()<<"**************true:";
|
|
// return true;
|
|
// }
|
|
if(abs(this->collisionX - pntImg.x)<=1 && abs(this->collisionY-pntImg.y)<=1)//设置1像素的容差
|
|
{//同一碰撞点
|
|
qDebug()<<"**************true:";
|
|
return true;
|
|
}
|
|
else
|
|
{//不是同一碰撞点，更新碰撞点记录
|
|
// qDebug()<<"******imgX:"<<pntImg.x<<" collisionX:"<<this->collisionX<<" imgY:"<<pntImg.y<<" collisionY:"<<this->collisionY;
|
|
this->collisionX = pntImg.x;
|
|
this->collisionY = pntImg.y;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
//重置预警
|
|
void CollisionDetectionThread::resetWarning()
|
|
{
|
|
this->highWarning = false;
|
|
this->middleWarning = false;
|
|
this->lowWarning = false;
|
|
}
|
|
|
|
//显示预警通知
|
|
void CollisionDetectionThread::showWarningNotify(int collisionTime,int collisionDist,int collisionHeight)
|
|
{
|
|
// qDebug()<<"*************************showWarningNotify:";
|
|
QString title;
|
|
QString message = "";
|
|
// QString message = QString(u8"前方大约") + QString::number(collisionTime) + QString("s ") + QString(u8"后有") + QString::number(collisionHeight) + QString("m ") + QString("地形障碍物 ");
|
|
message += "前方" + QString::number(collisionDist) + "m存在高度为" + QString::number(collisionHeight) + "m的地形障碍物;"
|
|
+ "大约" + QString::number(collisionTime) + "s到达;" + "请尽快提高飞行高度!";
|
|
// qDebug()<<"*************************message:"<<message;
|
|
QVariantMap data;
|
|
if(collisionTime>40)
|
|
{
|
|
if(lowWarning) return;
|
|
title = "三级碰撞黄色警告!";
|
|
data["icon"] = ":/Resources/WarningLevel3.png";
|
|
qDebug()<<"*********message3:"<<message;
|
|
emit showNotify(title,message,data);
|
|
this->lowWarning = true;
|
|
}else if(collisionTime>20)
|
|
{
|
|
if(middleWarning) return;
|
|
title = "二级碰撞橙色警告!";
|
|
data["icon"] = ":/Resources/WarningLevel2.png";
|
|
qDebug()<<"**********message2:"<<message;
|
|
emit showNotify(title,message,data);
|
|
this->middleWarning = true;
|
|
}else if(collisionTime<=20)
|
|
{
|
|
if(highWarning) return;
|
|
title = "一级碰撞红色警告!";
|
|
data["icon"] = ":/Resources/WarningLevel1.png";
|
|
qDebug()<<"***********message1:"<<message;
|
|
emit showNotify(title,message,data);
|
|
this->highWarning = true;
|
|
}
|
|
// notifyManager->notify(title,message,data);
|
|
}
|
|
|
|
|
|
|
|
/********************************测试线程:发送飞机实时位置************************************/
|
|
SendDataTestThread::SendDataTestThread(QObject *parent): QObject{parent}
|
|
{
|
|
|
|
}
|
|
|
|
void SendDataTestThread::working()
|
|
{
|
|
// qDebug()<<"***************************SendDataTestThread";
|
|
|
|
pt.setX(119.668);
|
|
pt.setY(32.23);
|
|
// int i = 0;
|
|
|
|
m_timer = new QTimer;
|
|
connect(m_timer,&QTimer::timeout,this,[=](){
|
|
generateData();
|
|
emit sendPositionData(lon,lat,relativeAlititude,heading,speed);
|
|
});
|
|
m_timer->start(200);
|
|
|
|
}
|
|
|
|
void SendDataTestThread::stopGenerateData()
|
|
{
|
|
m_timer->stop();
|
|
delete m_timer;
|
|
// this->deleteLater();
|
|
qDebug()<<"*******************************stopGenerateData";
|
|
}
|
|
|
|
void SendDataTestThread::generateData()
|
|
{
|
|
// qDebug() << "SendDataTestThread address: " << QThread::currentThread();
|
|
lon.clear();
|
|
lat.clear();
|
|
heading.clear();
|
|
speed.clear();
|
|
relativeAlititude.clear();
|
|
|
|
pt = geoComputation.getDestinationPoint(pt.x(),pt.y(),90,4);
|
|
// qDebug()<<"***********lon:"<<QString::number(pt.x(),'f',6)<<" lat:"<<QString::number(pt.y(),'f',6);
|
|
lon.append(pt.x());
|
|
lat.append(pt.y());
|
|
heading.append(90);
|
|
speed.append(20);
|
|
relativeAlititude.append(90);
|
|
}
|