//
|
// 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 "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"
|
|
using namespace std;
|
|
#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
|
|
enum {
|
TEST_NONE,
|
TEST_PARK_EDGE,
|
TEST_PARK_BOTTOM,
|
TEST_TUNE_90,
|
TEST_S_CURVE,
|
TEST_SLOPE,
|
TEST_SIMPLE,
|
TEST_END
|
};
|
|
static bool TestStart = false;
|
static int TestItem = TEST_NONE;
|
static int CarInArea = 0;
|
int errs = 0;
|
|
vector<int> ErrorList;
|
|
static Polygon theParkEdgeMap; // 侧位停车地图
|
static Polygon theTurn90Map;
|
static Polygon theSSMap;
|
|
#define CAR_COORD_STORE_SIZE 10
|
|
struct car_coord_ {
|
uint32_t uptime;
|
double azimuth;
|
PointF coord;
|
};
|
|
struct car_desc_ {
|
double distance; // 距离主天线的距离
|
double angle; // 从中轴线逆时针形成的角度
|
};
|
|
struct car_desc_ *CarDesc = NULL;
|
|
struct car_coord_ *CarCoord = NULL;
|
static double currSpeed = 0, currAzimuth = 0;
|
static PointF currCoord;
|
static int prevDirect = 0, currDirect = 0;
|
|
static int car_coord_num = 0, car_coord_in = 0;
|
|
#define MOV_AVG_SIZE 1
|
#define RTK_HISTORY_SIZE 1000
|
#define CAR_MODEL_CACHE_SIZE 10
|
|
struct rtk_info_ {
|
int qf;
|
time_t uptime;
|
double azimuth;
|
PointF coord;
|
};
|
|
static struct rtk_info_ *RTKHistory = NULL;
|
static int rtk_history_num = 0, rtk_history_in = 0;
|
static pthread_mutex_t add_rtk_history_mutex = PTHREAD_MUTEX_INITIALIZER;
|
|
static carModelDesc_t *carModelDescFile = NULL;
|
|
static car_model_cache_t carModelCache[CAR_MODEL_CACHE_SIZE];
|
static int carModelCacheIn, carModelCacheNum;
|
|
static void UpdateCarBodyCoord(double azimuth, PointF coord);
|
static void UpdateCarBodyCoord(double azimuth, PointF coord, car_model_cache_t *carModel, int &carModelIn, int &carModelNum);
|
static bool FrontTireEnterArea(const Polygon *car, const Polygon *map);
|
|
void DriverTestInit(void)
|
{
|
carModelDescFile = (carModelDesc_t *)malloc(sizeof(carModelDesc_t));
|
|
carModelDescFile->body_num = 6;
|
carModelDescFile->body[0] = 0;
|
carModelDescFile->body[1] = 1;
|
carModelDescFile->body[2] = 2;
|
carModelDescFile->body[3] = 3;
|
carModelDescFile->body[4] = 4;
|
carModelDescFile->body[5] = 5;
|
|
carModelDescFile->front_left_tire[0] = 1;
|
carModelDescFile->front_left_tire[1] = 1;
|
carModelDescFile->front_right_tire[0] = 5;
|
carModelDescFile->front_right_tire[1] = 5;
|
carModelDescFile->rear_left_tire[0] = 2;
|
carModelDescFile->rear_left_tire[1] = 2;
|
carModelDescFile->rear_right_tire[0] = 4;
|
carModelDescFile->rear_right_tire[1] = 4;
|
|
memset(&carModelCache, 0, sizeof(carModelCache));
|
carModelCacheIn = carModelCacheNum = 0;
|
|
CarDesc = (struct car_desc_ *) malloc(sizeof(struct car_desc_) * 6);
|
CarDesc[0].distance = 0.2465;
|
CarDesc[0].angle = 0;
|
|
CarDesc[1].distance = 0.2635;
|
CarDesc[1].angle = 20.7;
|
|
CarDesc[2].distance = 0.14;
|
CarDesc[2].angle = 138.9;
|
|
CarDesc[3].distance = 0.1055;
|
CarDesc[3].angle = 180.0;
|
|
CarDesc[4].distance = 0.14;
|
CarDesc[4].angle = 221.1;
|
|
CarDesc[5].distance = 0.2635;
|
CarDesc[5].angle = 339.3;
|
|
memset(&theParkEdgeMap, 0, sizeof(theParkEdgeMap));
|
theParkEdgeMap.num = 8;
|
theParkEdgeMap.point = (PointF *)malloc(theParkEdgeMap.num * sizeof(PointF));
|
|
theParkEdgeMap.point[0].Y = 28.013;
|
theParkEdgeMap.point[0].X = -11.9669;
|
|
theParkEdgeMap.point[1].Y = 27.137;
|
theParkEdgeMap.point[1].X = -11.5114;
|
|
theParkEdgeMap.point[2].Y = 27.5039;
|
theParkEdgeMap.point[2].X = -10.8069;
|
|
theParkEdgeMap.point[3].Y = 26.4212;
|
theParkEdgeMap.point[3].X = -10.2969;
|
|
theParkEdgeMap.point[4].Y = 26.8894;
|
theParkEdgeMap.point[4].X = -9.2102;
|
|
theParkEdgeMap.point[5].Y = 28.0027;
|
theParkEdgeMap.point[5].X = -9.6513;
|
|
theParkEdgeMap.point[6].Y = 28.3797;
|
theParkEdgeMap.point[6].X = -8.9758;
|
|
theParkEdgeMap.point[7].Y = 29.3232;
|
theParkEdgeMap.point[7].X = -9.5057;
|
|
memset(&theTurn90Map, 0, sizeof(theTurn90Map));
|
theTurn90Map.num = 6;
|
theTurn90Map.point = (PointF *)malloc(theTurn90Map.num * sizeof(PointF));
|
|
theTurn90Map.point[0].Y = 10;
|
theTurn90Map.point[0].X = 10;
|
|
theTurn90Map.point[1].Y = 12.5;
|
theTurn90Map.point[1].X = 10;
|
|
theTurn90Map.point[2].Y = 12.5;
|
theTurn90Map.point[2].X = 12.5;
|
|
theTurn90Map.point[3].Y = 15;
|
theTurn90Map.point[3].X = 12.5;
|
|
theTurn90Map.point[4].Y = 15;
|
theTurn90Map.point[4].X = 15;
|
|
theTurn90Map.point[5].Y = 10;
|
theTurn90Map.point[5].X = 15;
|
|
memset(&theSSMap, 0, sizeof(theSSMap));
|
theSSMap.num = 9;
|
theSSMap.point = (PointF *)malloc(theSSMap.num * sizeof(PointF));
|
|
theSSMap.point[0].Y = 10;
|
theSSMap.point[0].X = 10;
|
|
theSSMap.point[8].Y = 10;
|
theSSMap.point[8].X = 15;
|
|
theSSMap.point[1].Y = 10;
|
theSSMap.point[1].X = 11.5;
|
|
theSSMap.point[4].Y = 10;
|
theSSMap.point[4].X = 12.5;
|
|
theSSMap.point[5].Y = 10;
|
theSSMap.point[5].X = 13.5;
|
|
theSSMap.point[2].Y = 12;
|
theSSMap.point[2].X = 11.5;
|
|
theSSMap.point[3].Y = 12;
|
theSSMap.point[3].X = 12.5;
|
|
theSSMap.point[6].Y = 12;
|
theSSMap.point[6].X = 13.5;
|
|
theSSMap.point[7].Y = 12;
|
theSSMap.point[7].X = 15;
|
|
pthread_mutex_init(&add_rtk_history_mutex, NULL);
|
|
RTKHistory = (struct rtk_info_ *) malloc(RTK_HISTORY_SIZE * sizeof(struct rtk_info_));
|
rtk_history_num = rtk_history_in = 0;
|
|
CarCoord = (struct car_coord_ *) malloc(CAR_COORD_STORE_SIZE * sizeof(struct car_coord_));
|
car_coord_num = car_coord_in = 0;
|
|
TestStart = true;
|
TestItem = TEST_NONE;
|
|
ErrorList.clear();
|
|
TextSpeak("开始测试");
|
}
|
|
void UpdateRTKInfo(struct rtk_info *s)
|
{
|
struct tm test_tm;
|
|
struct timeval tv;
|
struct timezone tz;
|
|
gettimeofday(&tv,&tz);
|
|
memset(&test_tm, 0, sizeof(test_tm));
|
|
test_tm.tm_year = 2000 + s->YY - 1900;
|
test_tm.tm_mon = s->MM - 1;
|
test_tm.tm_mday = s->DD;
|
test_tm.tm_hour = s->hh;
|
test_tm.tm_min = s->mm;
|
test_tm.tm_sec = s->ss;
|
|
pthread_mutex_lock(&add_rtk_history_mutex);
|
/*if (s->qf == 3)*/ {
|
RTKHistory[rtk_history_in].qf = RTK_FIX;//s->qf;
|
RTKHistory[rtk_history_in].uptime = mktime(&test_tm) - tz.tz_minuteswest*60;
|
RTKHistory[rtk_history_in].azimuth = s->heading;
|
RTKHistory[rtk_history_in].coord.X = s->x;
|
RTKHistory[rtk_history_in].coord.Y = s->y;
|
|
rtk_history_in = (rtk_history_in + 1) % RTK_HISTORY_SIZE;
|
if (rtk_history_num < RTK_HISTORY_SIZE)
|
rtk_history_num++;
|
}
|
pthread_mutex_unlock(&add_rtk_history_mutex);
|
|
// DEBUG("UpdateRTKInfo qf = %d tm %ld", s->qf, mktime(&test_tm) - tz.tz_minuteswest*60);
|
}
|
|
void UpdateCarCoord(void)
|
{
|
double azimuth;
|
PointF coord;
|
|
// 计算最近的几个，滑动平均
|
int cnt = 0, valid_cnt = 0;
|
int s, sm;
|
time_t prev_time = 0;
|
time_t last_rtk_time = 0;
|
|
pthread_mutex_lock(&add_rtk_history_mutex);
|
s = rtk_history_in;
|
sm = rtk_history_num;
|
pthread_mutex_unlock(&add_rtk_history_mutex);
|
|
DEBUG("UpdateCarCoord rtk_history_in %d rtk_history_num %d car_coord_num %d", s, sm, car_coord_num);
|
|
// 如果出现QF不是固定解、GPS报文丢失的情况，就按上次的行驶状态做直线行驶
|
for (; cnt < sm && valid_cnt < MOV_AVG_SIZE; cnt++) {
|
if (s == 0) {
|
s = RTK_HISTORY_SIZE - 1;
|
} else {
|
s--;
|
}
|
|
if (cnt == 0) {
|
last_rtk_time = RTKHistory[s].uptime;
|
}
|
|
if (RTKHistory[s].uptime - last_rtk_time <= 10) {
|
if (RTKHistory[s].qf == RTK_FIX) {
|
if (valid_cnt != 0) {
|
azimuth += RTKHistory[s].azimuth;
|
coord.X += RTKHistory[s].coord.X;
|
coord.Y += RTKHistory[s].coord.Y;
|
} else {
|
azimuth = RTKHistory[s].azimuth;
|
coord.X = RTKHistory[s].coord.X;
|
coord.Y = RTKHistory[s].coord.Y;
|
}
|
valid_cnt++;
|
}
|
} else {
|
break;
|
}
|
prev_time = RTKHistory[s].uptime;
|
}
|
|
if (valid_cnt >= MOV_AVG_SIZE) {
|
azimuth /= MOV_AVG_SIZE;
|
coord.X /= MOV_AVG_SIZE;
|
coord.Y /= MOV_AVG_SIZE;
|
|
CarCoord[car_coord_in].uptime = AppTimer_GetTickCount();
|
|
CarCoord[car_coord_in].azimuth = azimuth;
|
CarCoord[car_coord_in].coord = coord;
|
car_coord_in = (car_coord_in + 1) % CAR_COORD_STORE_SIZE;
|
if (car_coord_num < CAR_COORD_STORE_SIZE)
|
car_coord_num++;
|
} else if (car_coord_num >= 3) {
|
// 按上次运行轨迹推算一步
|
int p1 = ((car_coord_in-1)+CAR_COORD_STORE_SIZE)%CAR_COORD_STORE_SIZE;
|
uint32_t tm = AppTimer_GetTickCount() - CarCoord[p1].uptime;
|
double distance = currSpeed * tm / 1000;
|
|
PointF xy;
|
|
xy.X = CarCoord[p1].coord.X + distance * sin(toRadians(CarCoord[p1].azimuth)) * currDirect;
|
xy.Y = CarCoord[p1].coord.Y + distance * sin(toRadians(CarCoord[p1].azimuth)) * currDirect;
|
|
CarCoord[car_coord_in].uptime = AppTimer_GetTickCount();
|
CarCoord[car_coord_in].azimuth = CarCoord[p1].azimuth;
|
CarCoord[car_coord_in].coord = xy;
|
car_coord_in = (car_coord_in + 1) % CAR_COORD_STORE_SIZE;
|
if (car_coord_num < CAR_COORD_STORE_SIZE)
|
car_coord_num++;
|
} else {
|
return;
|
}
|
|
// 计算车辆轮廓点、速度、前进后退
|
if (car_coord_num >= 3) {
|
int p1 = ((car_coord_in-1)+CAR_COORD_STORE_SIZE)%CAR_COORD_STORE_SIZE;
|
int p2 = ((car_coord_in-2)+CAR_COORD_STORE_SIZE)%CAR_COORD_STORE_SIZE;
|
int p3 = ((car_coord_in-3)+CAR_COORD_STORE_SIZE)%CAR_COORD_STORE_SIZE;
|
|
double speed1 = sqrt(pow(CarCoord[p1].coord.X - CarCoord[p2].coord.X, 2) + pow(CarCoord[p1].coord.Y - CarCoord[p2].coord.Y, 2)) * 1000 /
|
(double)(CarCoord[p1].uptime - CarCoord[p2].uptime);
|
double speed2 = sqrt(pow(CarCoord[p2].coord.X - CarCoord[p3].coord.X, 2) + pow(CarCoord[p2].coord.Y - CarCoord[p3].coord.Y, 2)) * 1000 /
|
(double)(CarCoord[p2].uptime - CarCoord[p3].uptime);
|
|
currSpeed = speed1;
|
currAzimuth = CarCoord[p1].azimuth;
|
currCoord = CarCoord[p1].coord;
|
|
DEBUG("方向 spd1 %f spd2 %f", speed1, speed2);
|
|
if (speed1 < 0.05 && speed2 < 0.05) {
|
// 停车
|
currDirect = 0;
|
// TextSpeak("停");
|
} else if (speed1 < 0.05) {
|
currDirect = prevDirect;
|
} else {
|
// 判断前进还是后退
|
double deg = 0.0;
|
|
if (fabs(CarCoord[p1].coord.Y-CarCoord[p2].coord.Y) <= GLB_EPSILON) {
|
if (CarCoord[p1].coord.X > CarCoord[p2].coord.X) {
|
deg = 90;
|
} else {
|
deg = 270;
|
}
|
|
DEBUG("方向 deg %f p1x %f p2x %f", deg, CarCoord[p1].coord.X, CarCoord[p2].coord.X);
|
} else if (fabs(CarCoord[p1].coord.X - CarCoord[p2].coord.X) <= GLB_EPSILON) {
|
if (CarCoord[p1].coord.Y > CarCoord[p2].coord.Y) {
|
deg = 0;
|
} else {
|
deg = 180;
|
}
|
} else {
|
deg = atan(fabs(CarCoord[p1].coord.X - CarCoord[p2].coord.X) /
|
fabs(CarCoord[p1].coord.Y - CarCoord[p2].coord.Y));
|
|
deg = toDegree(deg);
|
|
if (CarCoord[p1].coord.X > CarCoord[p2].coord.X &&
|
CarCoord[p1].coord.Y > CarCoord[p2].coord.Y) {
|
|
} else if (CarCoord[p1].coord.X < CarCoord[p2].coord.X &&
|
CarCoord[p1].coord.Y > CarCoord[p2].coord.Y) {
|
deg = 360 - deg;
|
} else if (CarCoord[p1].coord.X < CarCoord[p2].coord.X &&
|
CarCoord[p1].coord.Y < CarCoord[p2].coord.Y) {
|
deg = 180 + deg;
|
} else if (CarCoord[p1].coord.X > CarCoord[p2].coord.X &&
|
CarCoord[p1].coord.Y < CarCoord[p2].coord.Y) {
|
deg = 180 - deg;
|
}
|
|
DEBUG("方向 deg %f p1x %f p2x %f p1y %f p2y %f", deg, CarCoord[p1].coord.X,
|
CarCoord[p2].coord.X, CarCoord[p1].coord.Y, CarCoord[p2].coord.Y);
|
}
|
|
deg = fabs(CarCoord[p1].azimuth - deg);
|
if (deg > 180) {
|
deg = 360 - deg;
|
}
|
if (deg < 90) {
|
// 前进
|
currDirect = 1;
|
// TextSpeak("进");
|
} else {
|
// 后退
|
currDirect = -1;
|
// TextSpeak("退");
|
}
|
|
prevDirect = currDirect;
|
}
|
|
DEBUG("speed = %f, azimuth = %f coord.X = %f coord.Y = %f dir = %d", speed1, CarCoord[p1].azimuth,
|
CarCoord[p1].coord.X, CarCoord[p1].coord.Y, currDirect);
|
|
// UpdateCarBodyCoord(CarCoord[p1].azimuth, CarCoord[p1].coord);
|
UpdateCarBodyCoord(CarCoord[p1].azimuth, CarCoord[p1].coord, carModelCache, carModelCacheIn, carModelCacheNum);
|
// for (int i = 0; i < theCarModel.num; ++i) {
|
// DEBUG("n = %d X = %f Y = %f", i, theCarModel.point[i].X, theCarModel.point[i].Y);
|
// }
|
|
|
int c1 = ((carModelCacheIn-1)+CAR_MODEL_CACHE_SIZE)%CAR_MODEL_CACHE_SIZE;
|
Polygon py;
|
|
py.num = carModelCache[c1].point_num;
|
py.point = carModelCache[c1].points;
|
|
DrawScreen(&theSSMap, &py);
|
|
if (!TestStart) return;
|
|
if (CarInArea == 0) {
|
// if (FrontTireEnterArea(&py, &theParkEdgeMap)) {
|
// CarInArea = TEST_PARK_BOTTOM;
|
// TestItem = TEST_PARK_BOTTOM;
|
// StartParkBottom();
|
// }
|
|
CarInArea = TEST_SLOPE;
|
TestItem = TEST_SLOPE;
|
StartSAS();
|
}
|
|
switch (TestItem) {
|
case TEST_NONE: {
|
break;
|
}
|
case TEST_PARK_BOTTOM: {
|
errs = TestParkBottom(ErrorList, &theParkEdgeMap, GetCarModelCache(0), currSpeed, currDirect);
|
|
if (errs != 0) {
|
StopParkBottom();
|
TestItem = TEST_NONE;
|
}
|
break;
|
}
|
case TEST_PARK_EDGE: {
|
errs = TestParkEdge(ErrorList, &theParkEdgeMap, GetCarModelCache(0), currSpeed, currDirect);
|
|
if (errs != 0) {
|
StopParkEdge();
|
TestItem = TEST_NONE;
|
}
|
break;
|
}
|
|
case TEST_TUNE_90: {
|
errs = TestTurnA90(ErrorList, &theTurn90Map, GetCarModelCache(0), currSpeed, currDirect, currAzimuth);
|
|
if (errs != 0) {
|
StopTurnA90();
|
TestItem = TEST_NONE;
|
}
|
|
break;
|
}
|
|
case TEST_SLOPE: {
|
errs = TestSAS(ErrorList, &theSSMap, GetCarModelCache(0), currSpeed, currDirect);
|
|
if (errs != 0) {
|
StopSAS();
|
TestItem = TEST_NONE;
|
}
|
|
break;
|
}
|
default:
|
break;
|
}
|
|
if (ErrorList.size() > 0) {
|
vector<int>::iterator it = ErrorList.end();
|
it--;
|
error_list_t list = GetErrorList(*it);
|
|
int scr = 0;
|
|
for (vector<int>::iterator it1 = ErrorList.begin(); it1 != ErrorList.end(); ++it1) {
|
error_list_t list = GetErrorList(*it1);
|
|
scr += list.dec_score;
|
}
|
|
char buff[256];
|
|
sprintf(buff, "%s, 总计扣分 %d", list.text_desc, scr);
|
|
TextOsd(1, buff);
|
}
|
}
|
}
|
|
car_model_cache_t *GetCarModelCache(int node)
|
{
|
if (node < carModelCacheNum) {
|
node = ((carModelCacheIn-node-1)+CAR_MODEL_CACHE_SIZE)%CAR_MODEL_CACHE_SIZE;
|
return &carModelCache[node];
|
}
|
return NULL;
|
}
|
|
/*******************************************************************
|
* @brief 由主天线坐标计算车身点坐标
|
* @param azimuth
|
* @param coord
|
*/
|
static void UpdateCarBodyCoord(double azimuth, PointF coord, car_model_cache_t *carModel, int &in, int &num)
|
{
|
carModel[in].uptime = AppTimer_GetTickCount();
|
carModel[in].desc = carModelDescFile;
|
carModel[in].point_num = 6;
|
|
if (carModel[in].points == NULL) {
|
carModel[in].points = (PointF *) malloc(sizeof(PointF) * carModel[in].point_num);
|
}
|
|
for (int i = 0; i < 6; ++i) {
|
double tx = coord.X + CarDesc[i].distance*sin(toRadians(azimuth));
|
double ty = coord.Y + CarDesc[i].distance*cos(toRadians(azimuth));
|
|
carModel[in].points[i].X = (tx-coord.X)*cos(toRadians(CarDesc[i].angle)) -
|
(ty-coord.Y)*sin(toRadians(CarDesc[i].angle)) + coord.X;
|
carModel[in].points[i].Y = (tx-coord.X)*sin(toRadians(CarDesc[i].angle)) +
|
(ty-coord.Y)*cos(toRadians(CarDesc[i].angle)) + coord.Y;
|
}
|
|
in = (in + 1) % CAR_MODEL_CACHE_SIZE;
|
if (num < CAR_MODEL_CACHE_SIZE) {
|
num++;
|
}
|
}
|
|
static void UpdateCarBodyCoord(double azimuth, PointF coord)
|
{
|
// static double az = 0;
|
//
|
// az += 2.0;
|
//
|
// for (int i = 0; i < theCarModel.num; ++i) {
|
// double tx = coord.X + CarDesc[i].distance*sin(toRadians(azimuth));
|
// double ty = coord.Y + CarDesc[i].distance*cos(toRadians(azimuth));
|
//
|
// theCarModel.point[i].X = (tx-coord.X)*cos(toRadians(CarDesc[i].angle)) -
|
// (ty-coord.Y)*sin(toRadians(CarDesc[i].angle)) + coord.X;
|
// theCarModel.point[i].Y = (tx-coord.X)*sin(toRadians(CarDesc[i].angle)) +
|
// (ty-coord.Y)*cos(toRadians(CarDesc[i].angle)) + coord.Y;
|
// }
|
// UpdateParkTest(&theCarModel, &theTireModel, speed);
|
// UpdatePark2Test(&theCarModel, &theTireModel, speed);
|
// UpdateTuneAngle90Test(&theCarModel, 0);
|
//UpdateSlopeTest(&theCarModel, speed, direct);
|
}
|
|
static bool FrontTireEnterArea(const Polygon *car, const Polygon *map)
|
{
|
if (IntersectionOf(car->point[0], map) == GM_Containment) {
|
return true;
|
}
|
return false;
|
}
|
