//
|
// Created by YY on 2019/11/4.
|
//
|
|
#include "driving_curve.h"
|
#include "../jni_log.h"
|
#include "../driver_test.h"
|
#include "../common/apptimer.h"
|
#include "../utils/xconvert.h"
|
|
#include <vector>
|
#include <cstdlib>
|
|
using namespace std;
|
|
#define DEBUG(fmt, args...) LOGD("<driving_curve> <%s>: " fmt, __func__, ##args)
|
|
enum {
|
DRIVING_ON_CURVE
|
};
|
|
const uint32_t STOP_CAR_TIME = D_SEC(2);
|
|
static bool testing = false;
|
static uint32_t stopTimepoint = 0;
|
|
static bool reportStopCarTimeout;
|
static int prevMoveDirect;
|
static bool crashRedLine;
|
static struct calc_zone_t {
|
int leftStart;
|
int leftEnd;
|
int rightStart;
|
int rightEnd;
|
} calcZone;
|
|
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct calc_zone_t *zone);
|
|
void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime)
|
{
|
DEBUG("进入曲线行驶场地");
|
|
testing = true;
|
|
prevMoveDirect = moveDirect;
|
if (moveDirect == 0) {
|
stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
}
|
reportStopCarTimeout = false;
|
crashRedLine = false;
|
|
calcZone.leftStart = calcZone.leftEnd = calcZone.rightStart = calcZone.rightEnd = 0;
|
}
|
|
int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
Line start, end;
|
Line axial;
|
|
MakeLine(&axial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
|
|
MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]);
|
MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]);
|
|
if (IntersectionOf(start, axial) == GM_None) {
|
// 向起点查找
|
do {
|
if (calcZone.leftStart == 0 && calcZone.rightStart == 0) {
|
break;
|
}
|
if (calcZone.leftStart > 0)
|
calcZone.leftStart--;
|
if (calcZone.rightStart > 0)
|
calcZone.rightStart--;
|
MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]);
|
} while (IntersectionOf(start, axial) == GM_None);
|
} else {
|
// 向终点查找
|
do {
|
if (calcZone.leftStart >= map->num && calcZone.rightStart >= map2->num) {
|
break;
|
}
|
if (calcZone.leftStart < map->num)
|
calcZone.leftStart++;
|
if (calcZone.rightStart < map2->num)
|
calcZone.rightStart++;
|
MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]);
|
} while (IntersectionOf(start, axial) != GM_None);
|
}
|
|
if (IntersectionOf(end, axial) == GM_None) {
|
// 向终点查找
|
do {
|
if (calcZone.leftEnd >= map->num && calcZone.rightEnd >= map2->num) {
|
break;
|
}
|
if (calcZone.leftEnd < map->num)
|
calcZone.leftEnd++;
|
if (calcZone.rightEnd < map2->num)
|
calcZone.rightEnd++;
|
MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]);
|
} while (IntersectionOf(end, axial) == GM_None);
|
} else {
|
// 向起点查找
|
do {
|
if (calcZone.leftEnd == 0 && calcZone.rightEnd == 0) {
|
break;
|
}
|
if (calcZone.leftEnd > 0)
|
calcZone.leftEnd--;
|
if (calcZone.rightEnd > 0)
|
calcZone.rightEnd--;
|
MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]);
|
} while (IntersectionOf(end, axial) == GM_None);
|
}
|
|
if (calcZone.leftStart <= calcZone.leftEnd || calcZone.rightStart <= calcZone.rightEnd) {
|
// 离开场地
|
testing = false;
|
}
|
|
if (CrashRedLine(map, map2, car, &calcZone)) {
|
if (!crashRedLine) {
|
crashRedLine = true;
|
// 车轮压边线，不合格
|
AddExamFault(27, rtkTime);
|
DEBUG("车轮压边线");
|
}
|
} else {
|
crashRedLine = false;
|
}
|
|
if (moveDirect != prevMoveDirect) {
|
if (moveDirect == 0) {
|
stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
reportStopCarTimeout = false;
|
|
DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
|
} else {
|
|
}
|
prevMoveDirect = moveDirect;
|
} else if (moveDirect == 0) {
|
uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
|
if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarTimeout) {
|
// 停车超2秒，不合格
|
AddExamFault(28, rtkTime);
|
DEBUG("中途停车");
|
reportStopCarTimeout = true;
|
}
|
}
|
|
return testing ? 1 : 0;
|
}
|
|
// 车轮是否压边线
|
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct calc_zone_t *zone)
|
{
|
bool ret = false;
|
|
Line frontTireAxial, rearTireAxial;
|
Line redLine;
|
|
MakeLine(&frontTireAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
|
MakeLine(&rearTireAxial, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
|
|
int s = zone->leftStart;
|
for (int e = zone->leftStart + 1; e < zone->leftEnd; ++e) {
|
MakeLine(&redLine, &map->point[s], &map->point[e]);
|
if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
|
return true;
|
}
|
if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
|
return true;
|
}
|
s = e;
|
}
|
|
s = zone->rightStart;
|
for (int e = zone->rightStart + 1; e < zone->rightEnd; ++e) {
|
MakeLine(&redLine, &map2->point[s], &map2->point[e]);
|
if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
|
return true;
|
}
|
if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
|
return true;
|
}
|
s = e;
|
}
|
|
return ret;
|
}
|
