//
|
// Created by YY on 2019/10/21.
|
// Units note: distance - metre
|
// speed - metre per second
|
// angle - DEGREES
|
//
|
|
#include <cstdlib>
|
#include <cmath>
|
#include <semaphore.h>
|
#include <pthread.h>
|
#include <cstring>
|
#include <vector>
|
#include <list>
|
#include <numeric>
|
#include <algorithm>
|
|
#include "driver_test.h"
|
#include "defs.h"
|
#include "Geometry.h"
|
#include "common/apptimer.h"
|
#include "jni_log.h"
|
#include "test_items/park_edge.h"
|
#include "native-lib.h"
|
#include "test_items/park_bottom.h"
|
#include "test_items/park_edge.h"
|
#include "test_items/error_list.h"
|
#include "test_items/turn_a90.h"
|
#include "test_items/driving_curve.h"
|
#include "test_items/stop_and_start.h"
|
#include "master/comm_if.h"
|
#include "utils/xconvert.h"
|
#include "test_common/car_sensor.h"
|
#include "mcu/mcu_if.h"
|
#include "test_common/car_sensor.h"
|
|
#define DEBUG(fmt, args...) LOGD("<driver_test> <%s>: " fmt, __func__, ##args)
|
|
using namespace std;
|
|
#define MAP_TYPE_PARK_BUTTOM 1
|
#define MAP_TYPE_STOP_START 2
|
#define MAP_TYPE_PART_EDGE 3
|
#define MAP_TYPE_CURVE 4
|
#define MAP_TYPE_TURN_90 5
|
|
#define RTK_INVALID 0
|
#define RTK_SINGLE_POINT 1
|
#define RTK_DIFF 2
|
#define RTK_FIX 3
|
#define RTK_FLOAT 4
|
|
#define CAR_MODEL_POINT_NUM 32
|
#define MAP_LIST_SIZE 32
|
|
enum {
|
TEST_NONE,
|
TEST_PARK_EDGE,
|
TEST_PARK_BOTTOM,
|
TEST_TUNE_90,
|
TEST_S_CURVE,
|
TEST_SLOPE,
|
TEST_SIMPLE,
|
TEST_END
|
};
|
|
enum {
|
TEST_TYPE_AREA = 2,
|
TEST_TYPE_ROAD_DUMMY_LIGHT,
|
TEST_TYPE_ROAD_TRUE_LIGHT
|
};
|
|
static bool ExamStart = false;
|
static int ExamType;
|
static bool reportSeatbeltEject;
|
|
static int CarInArea = 0;
|
int errs = 0;
|
|
vector<int> ErrorList;
|
vector<ExamFault> ExamFaultList;
|
static int examFaultIndex = 0;
|
|
static struct map_list {
|
int id;
|
int type;
|
Polygon map;
|
Polygon map2;
|
} MapList[MAP_LIST_SIZE];
|
|
struct trigger_line {
|
int mapId;
|
Line tirgger;
|
};
|
|
struct road_exam_parent_map {
|
int id;
|
int type;
|
Polygon map;
|
int redLineNum;
|
Polygon *redLine;
|
int greenLineNum;
|
Polygon *greenLine;
|
int redAreaNum;
|
Polygon *redArea;
|
int triggerLineNum;
|
struct trigger_line *triggerLine;
|
};
|
|
static struct road_exam_parent_map * RoadParentMap;
|
|
struct road_exam_son_map {
|
int id;
|
int type;
|
char tts[512];
|
Line triggerLine;
|
Line controlLine;
|
Line targetLine;
|
int stop_flag;
|
|
int redLineNum;
|
Polygon *redLine;
|
};
|
|
typedef list<struct road_exam_son_map *> LIST_ROAD_SON_MAP;
|
static LIST_ROAD_SON_MAP RoadSonMapList;
|
|
static int MapNum = 0;
|
static int CurrExamMapIndex = -1;
|
static int CurrEnterMapIndex = -1;
|
|
static int CurrExamStatus = EXAM_AREA_NONE; // 1 测试完成 0 测试中 -1 测试错误退出
|
static int exam_dummy_light;
|
|
static car_model *CarModel = NULL;
|
|
typedef list<car_model *> LIST_CAR_MODEL;
|
|
static LIST_CAR_MODEL CarModelList;
|
|
static struct dummy_light_exam *DummyLightContent;
|
static int DummyLightContentSize;
|
static bool engineRuning = false;
|
const int ENGINE_MIN_ROTATE = 200;
|
static bool engineStart = false;
|
static bool engineStartTimeout = false;
|
|
#define MOV_AVG_SIZE 1
|
#define RTK_BUFFER_SIZE 100
|
#define CAR_MODEL_CACHE_SIZE 10
|
|
static rtk_info *RtkBuffer = NULL;
|
static int RtkBufferNum = 0, RtkBufferIn = 0;
|
|
static void DetectEnterOrExitMap(void);
|
static void EngineStartHold(union sigval sig);
|
static void ExecuteExam(const struct RtkTime* rtkTime);
|
static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime);
|
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b);
|
static void ReadDriverExamPrimerTimeout(union sigval sig);
|
static void UpdateCarBodyCoord(double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel);
|
static bool UpdateCarCoord(double &spd, int &mov, int &idx);
|
|
static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum);
|
static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum);
|
static int GetMapId(int index, const struct map_list *mapList, int mapNum);
|
static int GetMapType(int index, const struct map_list *mapList, int mapNum);
|
|
void DriverTestInit(void)
|
{
|
ExamStart = false;
|
memset(&MapList, 0, sizeof(MapList));
|
MapNum = 0;
|
CarModel = NULL;
|
CarModelList.clear();
|
|
RoadParentMap = NULL;
|
RoadSonMapList.clear();
|
|
CarSensorInit();
|
|
DummyLightContentSize = 0;
|
DummyLightContent = NULL;
|
|
RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
|
RtkBufferNum = RtkBufferIn = 0;
|
|
CurrExamStatus = EXAM_AREA_NONE;
|
}
|
|
static void ReadDriverExamPrimerTimeout(union sigval sig)
|
{
|
AppTimer_delete(ReadDriverExamPrimerTimeout);
|
AppTimer_add(ReadDriverExamPrimerTimeout, D_SEC(2));
|
ReadDriverExamPrimer();
|
}
|
|
void ReadDriverExamPrimer(void)
|
{
|
MA_ReadMap();
|
MA_ReadCar();
|
MA_ReadSensor();
|
}
|
|
void ClearMap(void)
|
{
|
if (ExamStart) return;
|
|
for (int i = 0; i < MapNum; ++i) {
|
if (MapList[i].map.point != NULL)
|
free(MapList[i].map.point);
|
if (MapList[i].map2.point != NULL)
|
free(MapList[i].map2.point);
|
}
|
memset(&MapList, 0, sizeof(MapList));
|
MapNum = 0;
|
}
|
|
void AddMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2)
|
{
|
DEBUG("加入地图信息 id %d type %d pointNum %d point2Num %d", id, type, pointNum, pointNum2);
|
|
AppTimer_delete(ReadDriverExamPrimerTimeout);
|
|
if (map == NULL || pointNum == 0 || ExamStart)
|
return;
|
|
MapList[MapNum].id = id;
|
|
MapList[MapNum].type = type;
|
|
MapList[MapNum].map.num = pointNum;
|
if (pointNum > 0) {
|
MapList[MapNum].map.point = (PointF *)malloc(sizeof(PointF) * pointNum);
|
for (int i = 0; i < pointNum; ++i) {
|
MapList[MapNum].map.point[i].X = map[i][0];
|
MapList[MapNum].map.point[i].Y = map[i][1];
|
}
|
}
|
|
MapList[MapNum].map2.num = pointNum2;
|
if (pointNum2 > 0 && map2 != NULL) {
|
MapList[MapNum].map2.point = (PointF *)malloc(sizeof(PointF) * pointNum2);
|
for (int i = 0; i < pointNum2; ++i) {
|
MapList[MapNum].map2.point[i].X = map2[i][0];
|
MapList[MapNum].map2.point[i].Y = map2[i][1];
|
}
|
}
|
|
MapNum++;
|
|
DEBUG("AddMap num %d", MapNum);
|
}
|
|
void AddRoadMapParent(int id, int type, const double (*points)[2], int pointNum, const int **redLine,
|
int redLineNum, const int **greenLine, int greenLineNum)
|
{
|
int id;
|
int type;
|
Polygon map;
|
int redLineNum;
|
Polygon *redLine;
|
int greenLineNum;
|
Polygon *greenLine;
|
int redAreaNum;
|
Polygon *redArea;
|
int triggerLineNum;
|
struct trigger_line *triggerLine;
|
|
RoadParentMap = (struct road_exam_parent_map *)malloc(sizeof(struct road_exam_parent_map));
|
RoadParentMap->id = id;
|
RoadParentMap->type = type;
|
|
RoadParentMap->map.num = pointNum;
|
if (pointNum > 0) {
|
RoadParentMap->map.point = (PointF *)malloc(sizeof(PointF) * pointNum);
|
for (int i = 0; i < pointNum; ++i) {
|
RoadParentMap->map.point[i].X = points[i][0];
|
RoadParentMap->map.point[i].Y = points[i][1];
|
}
|
}
|
|
RoadParentMap->redLineNum = redLineNum;
|
RoadParentMap->redLine = (Polygon *) malloc(sizeof(Polygon *));
|
|
for (int i = 0; i < redLineNum; ++i) {
|
RoadParentMap->redLine[i].num =
|
}
|
|
|
}
|
|
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
|
int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
|
int *body, int bodyNum,double (*point)[2], int pointNum, double antPitch)
|
{
|
DEBUG("加入车辆信息 pointNum %d", pointNum);
|
|
if (point == NULL || pointNum == 0 || ExamStart) return;
|
|
if (CarModel != NULL) {
|
if (CarModel->body != NULL)
|
free(CarModel->body);
|
if (CarModel->carDesc != NULL)
|
free(CarModel->carDesc);
|
if (CarModel->carXY != NULL)
|
free(CarModel->carXY);
|
free(CarModel);
|
CarModel = NULL;
|
}
|
|
CarModel = (car_model *)malloc(sizeof(car_model));
|
CarModel->basePoint.X = basePoint[0];
|
CarModel->basePoint.Y = basePoint[1];
|
CarModel->axial[0] = axial[0];
|
CarModel->axial[1] = axial[1];
|
CarModel->left_front_tire[0] = left_front_tire[0];
|
CarModel->left_front_tire[1] = left_front_tire[1];
|
CarModel->right_front_tire[0] = right_front_tire[0];
|
CarModel->right_front_tire[1] = right_front_tire[1];
|
CarModel->left_rear_tire[0] = left_rear_tire[0];
|
CarModel->left_rear_tire[1] = left_rear_tire[1];
|
CarModel->right_rear_tire[0] = right_rear_tire[0];
|
CarModel->right_rear_tire[1] = right_rear_tire[1];
|
|
CarModel->bodyNum = bodyNum;
|
if (bodyNum == 0 || body == NULL) {
|
CarModel->bodyNum = 6;
|
CarModel->body = (int *) malloc(sizeof(int) * 6);
|
for (int i = 0; i < 6; ++i) {
|
CarModel->body[i] = i;
|
}
|
} else {
|
CarModel->body = (int *) malloc(sizeof(int) * CarModel->bodyNum);
|
for (int i = 0; i < CarModel->bodyNum; ++i) {
|
CarModel->body[i] = body[i];
|
}
|
}
|
|
CarModel->antPitch = antPitch;
|
|
CarModel->pointNum = pointNum;
|
CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * pointNum);
|
CarModel->carXY = (PointF *) malloc(sizeof(PointF) * pointNum);
|
|
// 测量坐标转换为距离-角度形式
|
double C02 = (point[0][0]-basePoint[0])*(point[0][0]-basePoint[0]) +
|
(point[0][1]-basePoint[1])*(point[0][1]-basePoint[1]);
|
double C0 = sqrt(C02);
|
|
CarModel->carDesc[0].distance = sqrt(C02);
|
CarModel->carDesc[0].angle = 0.0;
|
|
for (int i = 1; i < pointNum; ++i) {
|
double dis2 = (point[i][0]-basePoint[0])*(point[i][0]-basePoint[0]) +
|
(point[i][1]-basePoint[1])*(point[i][1]-basePoint[1]);
|
double dis = sqrt(dis2);
|
|
CarModel->carDesc[i].distance = dis;
|
|
CarModel->carDesc[i].angle = 180 * acos((dis2 + C02 - ((point[i][0]-point[0][0])*(point[i][0]-point[0][0]) +
|
(point[i][1]-point[0][1])*(point[i][1]-point[0][1])))/(2*C0*dis)) / M_PI;
|
|
if (i > axial[1])
|
CarModel->carDesc[i].angle = 360.0 - CarModel->carDesc[i].angle;
|
|
DEBUG("加入点<%d> 距离 %f 角度 %f", i, CarModel->carDesc[i].distance, CarModel->carDesc[i].angle);
|
}
|
|
// CarModel->carDesc[0].distance = 0.2465;
|
// CarModel->carDesc[0].angle = 0;
|
//
|
// CarModel->carDesc[1].distance = 0.2635;
|
// CarModel->carDesc[1].angle = 20.7;
|
//
|
// CarModel->carDesc[2].distance = 0.14;
|
// CarModel->carDesc[2].angle = 138.9;
|
//
|
// CarModel->carDesc[3].distance = 0.1055;
|
// CarModel->carDesc[3].angle = 180.0;
|
//
|
// CarModel->carDesc[4].distance = 0.14;
|
// CarModel->carDesc[4].angle = 221.1;
|
//
|
// CarModel->carDesc[5].distance = 0.2635;
|
// CarModel->carDesc[5].angle = 339.3;
|
|
DEBUG("SetCarMeasurePoint Calc Over");
|
}
|
|
void SetDummyLightExam(int n, struct dummy_light_exam *cfg)
|
{
|
DEBUG("获取模拟路考灯光测试项目 N = %d %d", n, ExamStart);
|
|
// if (ExamStart) return;
|
|
if (DummyLightContent != NULL) {
|
free(DummyLightContent);
|
DummyLightContent = NULL;
|
}
|
|
DummyLightContent = (struct dummy_light_exam *)malloc(n * sizeof(struct dummy_light_exam));
|
DummyLightContentSize = n;
|
|
for (int i = 0; i < n; i++) {
|
DummyLightContent[i] = cfg[i];
|
}
|
}
|
|
void StartDriverExam(int start, int type)
|
{
|
bool err = false;
|
DEBUG("++++++++++++考试启动 start %d type %d+++++++++++++", start, type);
|
|
if (start == 0) {
|
DEBUG("结束考试");
|
// CurrExamMapIndex = -1;
|
// ExamStart = false;
|
// CommTestStart(false);
|
// MA_SendExamStatus(0, 0);
|
return;
|
}
|
|
if (MapNum == 0) {
|
err = true;
|
MA_SendExamStatus(0, -1);
|
}
|
if (CarModel == NULL) {
|
err = true;
|
MA_SendExamStatus(0, -2);
|
}
|
if (DummyLightContent == NULL && type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
|
err = true;
|
MA_SendExamStatus(0, -3);
|
}
|
|
if (!err) {
|
if (!ExamStart) {
|
DEBUG("启动考试");
|
ExamFaultList.clear();
|
examFaultIndex = 0;
|
|
ExamStart = true;
|
ExamType = type;
|
reportSeatbeltEject = false;
|
|
if (type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
|
exam_dummy_light = 0;
|
}
|
}
|
MA_SendExamStatus(1, 0);
|
}
|
}
|
|
void StartMapExam(int map_id, int exam)
|
{
|
DEBUG("测试该场地 %d: %d", map_id, exam);
|
|
if (map_id >= 0 && exam == 0) {
|
CurrExamMapIndex = map_id;
|
CurrExamStatus = EXAM_AREA_START;
|
}
|
}
|
|
void UpdateRTKInfo(const rtk_info *s)
|
{
|
struct RtkTime rtkClock;
|
|
rtkClock.YY = s->YY;
|
rtkClock.MM = s->MM;
|
rtkClock.DD = s->DD;
|
rtkClock.hh = s->hh;
|
rtkClock.mm = s->mm;
|
rtkClock.ss = s->ss;
|
rtkClock.mss = s->dss;
|
|
if (ExamStart) {
|
ExecuteExam(&rtkClock); // 执行无需车辆定位的项目
|
}
|
|
if (s->qf == 3) {
|
RtkBuffer[RtkBufferIn] = *s;
|
RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE;
|
if (RtkBufferNum < RTK_BUFFER_SIZE)
|
RtkBufferNum++;
|
} else {
|
return;
|
}
|
|
double speed;
|
int move;
|
int index;
|
|
if (UpdateCarCoord(speed, move, index)) {
|
struct carBrief brief;
|
|
sprintf(brief.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + RtkBuffer[index].YY,
|
RtkBuffer[index].MM, RtkBuffer[index].DD, RtkBuffer[index].hh,
|
RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);
|
|
brief.qf = RtkBuffer[index].qf;
|
brief.map_id = GetMapId(CurrExamMapIndex, MapList, MapNum);
|
brief.move = move;
|
brief.speed = speed * 3.6;
|
brief.heading = RtkBuffer[index].heading;
|
brief.main_ant[0] = RtkBuffer[index].x;
|
brief.main_ant[1] = RtkBuffer[index].y;
|
|
brief.axial[0] = CarModel->axial[0];
|
brief.axial[1] = CarModel->axial[1];
|
brief.left_front_tire[0] = CarModel->left_front_tire[0];
|
brief.left_front_tire[1] = CarModel->left_front_tire[1];
|
brief.right_front_tire[0] = CarModel->right_front_tire[0];
|
brief.right_front_tire[1] = CarModel->right_front_tire[1];
|
brief.left_rear_tire[0] = CarModel->left_rear_tire[0];
|
brief.left_rear_tire[1] = CarModel->left_rear_tire[1];
|
brief.right_rear_tire[0] = CarModel->right_rear_tire[0];
|
brief.right_rear_tire[1] = CarModel->right_rear_tire[1];
|
|
brief.bodyNum = CarModel->bodyNum;
|
brief.body = (int *) malloc(sizeof(int) * CarModel->bodyNum);
|
for (int i = 0; i < CarModel->bodyNum; ++i) {
|
brief.body[i] = CarModel->body[i];
|
}
|
|
brief.pointNum = CarModel->pointNum;
|
brief.point = (double *) malloc(CarModel->pointNum * 2 * sizeof(double));
|
for (int i = 0, j = 0; i < CarModel->pointNum; ++i) {
|
brief.point[j++] = CarModel->carXY[i].X;
|
brief.point[j++] = CarModel->carXY[i].Y;
|
}
|
|
MA_SendCarPosition(&brief);
|
|
free(brief.body);
|
free(brief.point);
|
|
struct RtkTime rtkTime;
|
double azimuth = RtkBuffer[index].heading;
|
|
rtkTime.YY = RtkBuffer[index].YY;
|
rtkTime.MM = RtkBuffer[index].MM;
|
rtkTime.DD = RtkBuffer[index].DD;
|
rtkTime.hh = RtkBuffer[index].hh;
|
rtkTime.mm = RtkBuffer[index].mm;
|
rtkTime.ss = RtkBuffer[index].ss;
|
rtkTime.mss = RtkBuffer[index].dss;
|
|
DetectEnterOrExitMap();
|
|
if (ExamStart) {
|
ExecuteExam(speed, move, azimuth, &rtkTime);
|
}
|
}
|
}
|
|
static void DetectEnterOrExitMap(void)
|
{
|
if (ExamType == TEST_TYPE_AREA) {
|
|
}
|
if (CurrExamMapIndex < 0) {
|
if (CurrEnterMapIndex < 0) {
|
CurrEnterMapIndex = EnterMap(CarModel, MapList, MapNum);
|
if (CurrEnterMapIndex >= 0) {
|
DEBUG("发送进入场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
|
MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1);
|
|
CurrExamMapIndex = CurrEnterMapIndex;
|
CurrExamStatus = EXAM_AREA_START;
|
}
|
} else {
|
if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) {
|
DEBUG("发送离开场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
|
MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0);
|
CurrEnterMapIndex = -1;
|
}
|
}
|
}
|
}
|
|
static void ExecuteExam(const struct RtkTime* rtkTime)
|
{
|
if (ReadCarStatus(ENGINE_RPM) < ENGINE_MIN_ROTATE) {
|
if (engineRuning) {
|
engineRuning = false;
|
if (ExamType == TEST_TYPE_AREA) {
|
// 熄火1次，扣10分
|
AddExamFault(5, rtkTime);
|
} else {
|
AddExamFault(20, rtkTime);
|
}
|
}
|
} else {
|
engineRuning = true;
|
}
|
|
if (ReadCarStatus(ENGINE_START) == ENGINE_START_ACTIVE) {
|
if (!engineStart) {
|
engineStart = true;
|
if (ReadCarStatus(SHIFT) != 'N') {
|
// 不是空挡点火，不合格
|
if (ExamType == TEST_TYPE_AREA)
|
AddExamFault(3, rtkTime);
|
else
|
AddExamFault(4, rtkTime);
|
}
|
AppTimer_delete(EngineStartHold);
|
AppTimer_add(EngineStartHold, D_SEC(2));
|
}
|
} else if (engineStart) {
|
engineStart = false;
|
AppTimer_delete(EngineStartHold);
|
}
|
|
if (engineStartTimeout) {
|
engineStartTimeout = false;
|
|
// 不及时松开启动开关，扣10分
|
if (ExamType == TEST_TYPE_AREA)
|
AddExamFault(4, rtkTime);
|
}
|
|
if (ExamType == TEST_TYPE_ROAD_DUMMY_LIGHT) {
|
if (exam_dummy_light == 0) {
|
StartDummyLightExam(DummyLightContent, DummyLightContentSize, rtkTime);
|
exam_dummy_light = 1;
|
// 汇报灯光考试开始
|
} else if (exam_dummy_light == 1) {
|
exam_dummy_light = ExecuteDummyLightExam(rtkTime);
|
// 汇报灯光考试结束
|
}
|
}
|
}
|
|
static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime)
|
{
|
if (move != 0) {
|
if (ReadCarStatus(SEATBELT) == EJECT_SEATBELT && !reportSeatbeltEject) {
|
DEBUG("不系安全带");
|
reportSeatbeltEject = true;
|
AddExamFault(1, rtkTime);
|
}
|
}
|
|
if (CurrExamMapIndex >= 0) {
|
int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
|
|
if (CurrExamStatus == EXAM_AREA_START) {
|
DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype);
|
|
switch (mtype) {
|
case MAP_TYPE_PARK_BUTTOM:
|
DEBUG("进入倒车入库场地");
|
MA_SendDebugInfo("进入倒车入库场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
|
|
StartParkBottom(move, rtkTime);
|
CurrExamStatus = EXAM_AREA_RUN;
|
break;
|
case MAP_TYPE_STOP_START:
|
DEBUG("进入上坡起步场地");
|
MA_SendDebugInfo("进入上坡起步场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
|
StartSAS(move, rtkTime);
|
CurrExamStatus = EXAM_AREA_RUN;
|
break;
|
case MAP_TYPE_PART_EDGE:
|
DEBUG("进入侧方位停车场地");
|
MA_SendDebugInfo("进入侧方位停车场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
|
StartParkEdge(move, rtkTime);
|
|
CurrExamStatus = EXAM_AREA_RUN;
|
break;
|
case MAP_TYPE_CURVE:
|
DEBUG("进入曲线行驶场地");
|
MA_SendDebugInfo("进入曲线行驶场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
|
StartDrivingCurve(move, rtkTime);
|
|
CurrExamStatus = EXAM_AREA_RUN;
|
break;
|
case MAP_TYPE_TURN_90:
|
DEBUG("进入直角转弯场地");
|
MA_SendDebugInfo("进入直角转弯场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
|
StartTurnA90(move, azimuth, rtkTime);
|
CurrExamStatus = EXAM_AREA_RUN;
|
break;
|
default:break;
|
}
|
} else if (CurrExamStatus == EXAM_AREA_RUN) {
|
int testing = 0;
|
switch (mtype) {
|
case MAP_TYPE_PARK_BUTTOM:
|
testing = TestParkBottom(&MapList[CurrExamMapIndex].map,
|
CarModel, NULL, speed, move, rtkTime);
|
break;
|
case MAP_TYPE_STOP_START:
|
testing = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, NULL, speed, move, rtkTime);
|
break;
|
case MAP_TYPE_PART_EDGE:
|
testing = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, NULL, speed, move, rtkTime);
|
break;
|
case MAP_TYPE_CURVE:
|
testing = TestDrivingCurve(&MapList[CurrExamMapIndex].map, &MapList[CurrExamMapIndex].map2, CarModel, NULL, speed, move, rtkTime);
|
break;
|
case MAP_TYPE_TURN_90:
|
testing = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, NULL, azimuth, speed, move, rtkTime);
|
break;
|
default:
|
break;
|
}
|
|
if (testing > 0) {
|
CurrExamStatus = EXAM_AREA_RUN;
|
} else {
|
CurrExamStatus = EXAM_AREA_END;
|
}
|
}
|
if (CurrExamStatus != EXAM_AREA_RUN) {
|
// 某项结束
|
//DEBUG("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
|
//MA_SendDebugInfo("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
|
CurrExamStatus = EXAM_AREA_NONE;
|
CurrExamMapIndex = -1;
|
}
|
}
|
}
|
|
static void EngineStartHold(union sigval sig) {
|
AppTimer_delete(EngineStartHold);
|
|
if (ReadCarStatus(ENGINE_START) == ENGINE_START_ACTIVE) {
|
engineStartTimeout = true;
|
}
|
}
|
|
/*************************************************
|
* 2次采样相差的时间, a 最近的，b 先前的
|
* @param a
|
* @param b
|
* @return ms
|
*/
|
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b)
|
{
|
return TimeGetDiff(a->hh, a->mm, a->ss, a->dss*10, b->hh, b->mm, b->ss, b->dss*10);
|
}
|
|
static bool UpdateCarCoord(double &spd, int &mov, int &idx)
|
{
|
long tmDiff;
|
|
if (CarModel == NULL)
|
return false;
|
|
if (RtkBufferNum < 2)
|
return false;
|
|
int p1 = ((RtkBufferIn-1)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE; // 最近采样值
|
int p2 = ((RtkBufferIn-2)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE; // 找到1秒前的采样值
|
int pn = 0;
|
|
for (pn = 1; pn < RtkBufferNum; ++pn) {
|
p2 = ((RtkBufferIn-1-pn)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
|
if ((tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2])) >= D_SEC(1)) break;
|
}
|
|
if (pn == RtkBufferNum)
|
return false;
|
|
// 如果一定的时间都没有有效定位，删除之前的值
|
/*tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2]);
|
|
DEBUG("tmDiff = %ld, p1 = %d p2 = %d dss = %d dss2 = %d", tmDiff, p1, p2, RtkBuffer[p1].dss, RtkBuffer[p2].dss);
|
|
if (tmDiff > D_SEC(5)) {
|
if (p1 != 0)
|
RtkBuffer[0] = RtkBuffer[p1];
|
RtkBufferIn = RtkBufferNum = 1;
|
return false;
|
}*/
|
|
// 计算车辆轮廓点
|
PointF main_ant_coord;
|
main_ant_coord.X = RtkBuffer[p1].x;
|
main_ant_coord.Y = RtkBuffer[p1].y;
|
|
UpdateCarBodyCoord(RtkBuffer[p1].heading, RtkBuffer[p1].pitch, RtkBuffer[p1].roll, main_ant_coord, CarModel);
|
|
car_model *newModel = (car_model *)malloc(sizeof(car_model));
|
|
newModel->basePoint = CarModel->basePoint;
|
newModel->axial[0] = CarModel->axial[0];
|
newModel->axial[1] = CarModel->axial[1];
|
newModel->left_front_tire[0] = CarModel->left_front_tire[0];
|
newModel->left_front_tire[1] = CarModel->left_front_tire[1];
|
newModel->right_front_tire[0] = CarModel->right_front_tire[0];
|
newModel->right_front_tire[1] = CarModel->right_front_tire[1];
|
newModel->left_rear_tire[0] = CarModel->left_rear_tire[0];
|
newModel->left_rear_tire[1] = CarModel->left_rear_tire[1];
|
newModel->right_rear_tire[0] = CarModel->right_rear_tire[0];
|
newModel->right_rear_tire[1] = CarModel->right_rear_tire[1];
|
newModel->bodyNum = CarModel->bodyNum;
|
newModel->body = (int *) malloc(sizeof(int) * newModel->bodyNum);
|
for (int i = 0; i < newModel->bodyNum; ++i) {
|
newModel->body[i] = CarModel->body[i];
|
}
|
newModel->pointNum = CarModel->pointNum;
|
newModel->carXY = (PointF *) malloc(sizeof(PointF) * newModel->pointNum);
|
for (int i = 0; i < newModel->pointNum; ++i) {
|
newModel->carXY[i] = CarModel->carXY[i];
|
}
|
newModel->carDesc = NULL;
|
newModel->antPitch = CarModel->antPitch;
|
newModel->yaw = CarModel->yaw;
|
newModel->pitch = CarModel->pitch;
|
|
CarModelList.push_front(newModel);
|
while (CarModelList.size() > 25) {
|
DEBUG("删除尾结点");
|
car_model *ptr = CarModelList.back();
|
|
if (ptr->body != NULL)
|
free(ptr->body);
|
if (ptr->carXY != NULL)
|
free(ptr->carXY);
|
if (ptr->carDesc != NULL)
|
free(ptr->carDesc);
|
|
free(ptr);
|
|
CarModelList.pop_back();
|
}
|
|
// 计算速度(米/秒)、前进后退
|
double speed = sqrt(pow(RtkBuffer[p1].x - RtkBuffer[p2].x, 2) + pow(RtkBuffer[p1].y - RtkBuffer[p2].y, 2)) * 1000 /
|
(double)(tmDiff);
|
int move = 0;
|
double deg = 0.0;
|
|
if (speed < 0.05) {
|
// 停车
|
move = 0;
|
} else {
|
// 判断前进还是后退
|
if (fabs(RtkBuffer[p1].y - RtkBuffer[p2].y) <= GLB_EPSILON) {
|
if (RtkBuffer[p1].x > RtkBuffer[p2].x) {
|
deg = 90;
|
} else {
|
deg = 270;
|
}
|
} else if (fabs(RtkBuffer[p1].x - RtkBuffer[p2].x) <= GLB_EPSILON) {
|
if (RtkBuffer[p1].y > RtkBuffer[p2].y) {
|
deg = 0;
|
} else {
|
deg = 180;
|
}
|
} else {
|
deg = atan(fabs(RtkBuffer[p1].x - RtkBuffer[p2].x) /
|
fabs(RtkBuffer[p1].y - RtkBuffer[p2].y));
|
|
deg = toDegree(deg);
|
|
if (RtkBuffer[p1].x > RtkBuffer[p2].x &&
|
RtkBuffer[p1].y > RtkBuffer[p2].y) {
|
|
} else if (RtkBuffer[p1].x < RtkBuffer[p2].x &&
|
RtkBuffer[p1].y > RtkBuffer[p2].y) {
|
deg = 360 - deg;
|
} else if (RtkBuffer[p1].x < RtkBuffer[p2].x &&
|
RtkBuffer[p1].y < RtkBuffer[p2].y) {
|
deg = 180 + deg;
|
} else if (RtkBuffer[p1].x > RtkBuffer[p2].x &&
|
RtkBuffer[p1].y < RtkBuffer[p2].y) {
|
deg = 180 - deg;
|
}
|
}
|
|
deg = fabs(RtkBuffer[p1].heading - deg);
|
if (deg > 180) {
|
deg = 360 - deg;
|
}
|
if (deg < 90) {
|
// 前进
|
move = 1;
|
} else {
|
// 后退
|
move = -1;
|
}
|
}
|
|
spd = speed;
|
mov = move;
|
idx = p1;
|
// DEBUG("tmDiff = %ld speed = %f m/Sec move = %d", tmDiff, speed, move);
|
|
return true;
|
}
|
|
static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum)
|
{
|
// 车的最前点是否进入地图
|
for (int i = 0; i < mapNum && car != NULL; ++i) {
|
if (mapList[i].type == MAP_TYPE_STOP_START) {
|
// 构造虚拟的左上角点
|
double x9, y9, xo, yo;
|
|
bool enter = false;
|
|
xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2;
|
yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2;
|
|
x9 = 2*xo - mapList[i].map.point[8].X;
|
y9 = 2*yo - mapList[i].map.point[8].Y;
|
|
Polygon map;
|
|
map.num = 4;
|
map.point = (PointF *) malloc(map.num * sizeof(PointF));
|
|
map.point[0] = mapList[i].map.point[0];
|
map.point[1] = mapList[i].map.point[8];
|
map.point[2] = mapList[i].map.point[7];
|
map.point[3].X = x9;
|
map.point[3].Y = y9;
|
|
if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &map) == GM_Containment) {
|
Line enterLine1;
|
|
MakeLine(&enterLine1, &(map.point[0]), &(map.point[3]));
|
|
if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
|
enter = true;
|
}
|
|
free(map.point);
|
|
if (enter) return i;
|
}
|
if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
|
if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
|
Line enterLine1, enterLine2;
|
|
MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
|
MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
|
|
if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
|
DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
|
return i;
|
}
|
}
|
if (mapList[i].type == MAP_TYPE_PART_EDGE) {
|
if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
|
Line enterLine;
|
|
MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
|
|
if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1)
|
return i;
|
}
|
}
|
if (mapList[i].type == MAP_TYPE_TURN_90) {
|
if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
|
Line enterLine1;
|
|
MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
|
|
if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
|
return i;
|
}
|
}
|
if (mapList[i].type == MAP_TYPE_CURVE) {
|
Line startLine;
|
Line carAxial;
|
|
MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]);
|
MakeLine(&carAxial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
|
|
if (IntersectionOf(startLine, carAxial) == GM_Intersection) {
|
if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], startLine) > 0.1)
|
return i;
|
}
|
}
|
}
|
return -1;
|
}
|
|
static bool ExitMap(const car_model *car, int index, const struct map_list *mapList, int mapNum)
|
{
|
bool ret = false;
|
if (index < 0 || mapList == NULL || mapNum == 0) return true;
|
|
if (mapList[index].type == MAP_TYPE_PARK_BUTTOM ||
|
mapList[index].type == MAP_TYPE_PART_EDGE ||
|
mapList[index].type == MAP_TYPE_TURN_90) {
|
// 全车都需不在地图中
|
Polygon carBody;
|
|
carBody.num = car->bodyNum;
|
carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
|
for (int i = 0; i < carBody.num; ++i) {
|
carBody.point[i] = car->carXY[car->body[i]];
|
}
|
|
if (IntersectionOf(&carBody, &mapList[index].map) == GM_None) {
|
ret = true;
|
}
|
|
free(carBody.point);
|
}
|
if (mapList[index].type == MAP_TYPE_STOP_START) {
|
// 构造虚拟的左上角点
|
double x9, y9, xo, yo;
|
|
bool enter = false;
|
|
xo = (mapList[index].map.point[0].X + mapList[index].map.point[7].X) / 2;
|
yo = (mapList[index].map.point[0].Y + mapList[index].map.point[7].Y) / 2;
|
|
x9 = 2*xo - mapList[index].map.point[8].X;
|
y9 = 2*yo - mapList[index].map.point[8].Y;
|
|
Polygon map;
|
|
map.num = 4;
|
map.point = (PointF *) malloc(map.num * sizeof(PointF));
|
|
map.point[0] = mapList[index].map.point[0];
|
map.point[1] = mapList[index].map.point[8];
|
map.point[2] = mapList[index].map.point[7];
|
map.point[3].X = x9;
|
map.point[3].Y = y9;
|
|
// 全车都需不在地图中
|
Polygon carBody;
|
|
carBody.num = car->bodyNum;
|
carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
|
for (int i = 0; i < carBody.num; ++i) {
|
carBody.point[i] = car->carXY[car->body[i]];
|
}
|
|
if (IntersectionOf(&carBody, &map) == GM_None) {
|
ret = true;
|
}
|
|
free(carBody.point);
|
free(map.point);
|
}
|
if (mapList[index].type == MAP_TYPE_CURVE) {
|
ret = ExitDrivingCurveArea(&mapList[index].map, &mapList[index].map2, car);
|
}
|
|
return ret;
|
}
|
|
static int GetMapId(int index, const struct map_list *mapList, int mapNum)
|
{
|
if (index < 0 || mapList == NULL || mapNum == 0)
|
return -1;
|
|
return mapList[index].id;
|
}
|
|
static int GetMapType(int index, const struct map_list *mapList, int mapNum)
|
{
|
if (index < 0 || mapList == NULL || mapNum == 0)
|
return -1;
|
|
return mapList[index].type;
|
}
|
|
void AddExamFault(int wrong, const struct RtkTime *rtkTime)
|
{
|
struct ExamFault fault;
|
|
if (!ExamStart)
|
return;
|
|
fault.sn = examFaultIndex++;
|
sprintf(fault.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + rtkTime->YY,
|
rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
|
fault.wrong_id = wrong;
|
|
DEBUG("考试发生错误 %d %s", wrong, fault.utc);
|
|
ExamFaultList.push_back(fault);
|
|
MA_SendExamWrong(ExamFaultList);
|
|
ExamFaultList.clear();
|
}
|
|
car_model_cache_t *GetCarModelCache(int node)
|
{
|
return NULL;
|
}
|
|
/*******************************************************************
|
* @brief 由主天线坐标计算车身点坐标
|
* @param azimuth
|
* @param coord
|
*/
|
static void UpdateCarBodyCoord(double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel)
|
{
|
carModel->basePoint = main_ant;
|
|
carModel->yaw = azimuth;
|
carModel->pitch = pitch;
|
|
pitch = pitch - carModel->antPitch;
|
|
for (int i = 0; i < carModel->pointNum; ++i) {
|
double qrx = carModel->carDesc[i].distance * sin(toRadians(carModel->carDesc[i].angle));
|
double qry =
|
carModel->carDesc[i].distance * cos(toRadians(carModel->carDesc[i].angle)) *
|
cos(toRadians(pitch));
|
|
double projectDistance = sqrt(pow(qrx, 2) + pow(qry, 2));
|
double projectAngle = toDegree(acos(qry / projectDistance));
|
|
if (carModel->carDesc[i].angle > 180) {
|
projectAngle = 360 - projectAngle;
|
}
|
|
// double tx = projectDistance*sin(toRadians(azimuth));
|
// double ty = projectDistance*cos(toRadians(azimuth));
|
|
carModel->carXY[i].X =
|
projectDistance * sin(toRadians(azimuth)) * cos(toRadians(projectAngle)) -
|
projectDistance * cos(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
|
main_ant.X;
|
carModel->carXY[i].Y =
|
projectDistance * sin(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
|
projectDistance * cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) +
|
main_ant.Y;
|
}
|
}
|
|
void SystemShutdown(int event, int timeout)
|
{
|
// 关机
|
if (event == 1) {
|
ShutdownInd(timeout);
|
}
|
// 重启
|
if (event == 0) {
|
|
}
|
}
|
