//
|
// Created by YY on 2020/3/23.
|
//
|
|
#include "drive_straight.h"
|
#include "../driver_test.h"
|
#include "../native-lib.h"
|
#include "../jni_log.h"
|
#include "road_exam.h"
|
#include "../master/comm_if.h"
|
#include <cmath>
|
|
#define DEBUG(fmt, args...) LOGD("<drive_straight> <%s>: " fmt, __func__, ##args)
|
|
static const double CHECK_STAGE_DISTANCE = 100.0;
|
static const double MAX_OFFSET_DISTANCE = 0.3;
|
|
static int ttsPlayEnd;
|
static bool crossStartLine;
|
static bool reportOffsetOver;
|
static double edgeDistance;
|
static double distanceToStartSum;
|
|
static double CalcDistance2Edge(road_t &road, const car_model *car);
|
|
void StartDriveStraightExam(std::string tts) {
|
DEBUG("开始直线行驶");
|
|
if (!tts.empty()) {
|
PlayTTS(tts.c_str());
|
} else {
|
DEBUG("没有TTS");
|
}
|
|
distanceToStartSum = 0;
|
reportOffsetOver = false;
|
}
|
|
int ExecuteDriveStraightExam(road_t &road, const car_model *car,
|
LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) {
|
static PointF startPoint;
|
|
double dis2roadEdge = 0;
|
|
if (ttsPlayEnd == 1) {
|
ttsPlayEnd = 2;
|
startPoint = car->basePoint;
|
edgeDistance = CalcDistance2Edge(road, car); // 基准边距
|
|
DEBUG("当前基准路边间距 %f", edgeDistance);
|
}
|
|
if (ttsPlayEnd != 2)
|
return 1;
|
|
double distanceToStart = DistanceOf(car->basePoint, startPoint);
|
dis2roadEdge = CalcDistance2Edge(road, car);
|
|
DEBUG("路边间距 %f --- %f", dis2roadEdge, edgeDistance);
|
|
if (!reportOffsetOver && fabs(dis2roadEdge - edgeDistance) > MAX_OFFSET_DISTANCE) {
|
DEBUG("直线偏移大于30厘米");
|
// 偏移大于30厘米，不合格
|
AddExamFault(30, rtkTime);
|
reportOffsetOver = true;
|
|
//////////////////////////////////////////////
|
startPoint = car->basePoint;
|
edgeDistance = dis2roadEdge;
|
reportOffsetOver = false;
|
}
|
|
if (distanceToStart > CHECK_STAGE_DISTANCE) {
|
DEBUG("复位边距偏移量");
|
startPoint = car->basePoint;
|
edgeDistance = dis2roadEdge;
|
reportOffsetOver = false;
|
distanceToStartSum += distanceToStart;
|
distanceToStart = 0;
|
}
|
|
if (distanceToStart + distanceToStartSum > 150) {
|
DEBUG("离开直线行驶区域");
|
return -1;
|
}
|
return 1;
|
}
|
|
static double CalcDistance2Edge(road_t &road, const car_model *car)
|
{
|
PointF vp;
|
bool get_vp = false;
|
double distance = 0;
|
|
// 检测道路边缘线
|
for (int i = 0; i < road.leftEdge.size(); ++i) {
|
PointF p1, p2;
|
Line edge;
|
|
p1 = road.leftEdge[i].points[0];
|
for (int j = 1; j < road.leftEdge[i].points.size(); ++j) {
|
p2 = road.leftEdge[i].points[j];
|
MakeLine(&edge, &p1, &p2);
|
|
if (VerticalPointOnLine(car->basePoint, edge, vp)) {
|
get_vp = true;
|
goto FIND_VP_END;
|
}
|
|
p1 = p2;
|
}
|
}
|
|
FIND_VP_END:
|
if (get_vp) {
|
distance = DistanceOf(car->basePoint, vp);
|
} else {
|
// 没有找到匹配线端，按最小距离顶点计算
|
distance = 100;
|
for (int i = 0; i < road.leftEdge.size(); ++i) {
|
for (int j = 0; j < road.leftEdge[i].points.size(); ++j) {
|
double x;
|
if (distance > (x = DistanceOf(car->basePoint, road.leftEdge[i].points[j]))) {
|
distance = x;
|
}
|
}
|
}
|
}
|
|
return distance;
|
}
|
