From cc4be69e76709579c80efcc24801004461011c46 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期二, 14 一月 2020 11:17:09 +0800
Subject: [PATCH] s
---
lib/src/main/cpp/driver_test.cpp | 560 +++++++++++++++----------------------------------------
1 files changed, 157 insertions(+), 403 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 6232383..28b5bfc 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -25,6 +25,7 @@
#include "test_items/driving_curve.h"
#include "test_items/stop_and_start.h"
#include "master/comm_if.h"
+#include "utils/xconvert.h"
#define DEBUG(fmt, args...) LOGD("<driver_test> <%s>: " fmt, __func__, ##args)
@@ -95,40 +96,22 @@
int right_rear_tire[2];
int pointNum;
struct car_desc_ *carDesc;
+ PointF *carXY;
} car_model;
static car_model *CarModel = 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 RTK_BUFFER_SIZE 100
#define CAR_MODEL_CACHE_SIZE 10
-struct rtk_info_ {
- int qf;
- time_t uptime;
- double azimuth;
- PointF coord;
-};
+static rtk_info *RtkBuffer = NULL;
+static int RtkBufferNum = 0, RtkBufferIn = 0;
-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 uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b);
static void ReadDriverExamPrimerTimeout(union sigval sig);
-static void UpdateCarBodyCoord(double azimuth, PointF coord);
-static void UpdateCarBodyCoord(double azimuth, PointF coord, car_model_cache_t *carModel, int &carModelIn, int &carModelNum);
+static void UpdateCarBodyCoord(double azimuth, PointF main_ant, const car_model *carModel);
+static bool UpdateCarCoord(double &spd, int &mov, int &idx);
static bool FrontTireEnterArea(const Polygon *car, const Polygon *map);
void DriverTestInit(void)
@@ -137,142 +120,8 @@
MapNum = 0;
CarModel = NULL;
- 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("寮�濮嬫祴璇�");
+ RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
+ RtkBufferNum = RtkBufferIn = 0;
}
static void ReadDriverExamPrimerTimeout(union sigval sig)
@@ -332,6 +181,8 @@
}
MapNum++;
+
+ DEBUG("AddMap num %d", MapNum);
}
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
@@ -343,13 +194,15 @@
if (point == NULL || pointNum == 0) return;
if (CarModel != NULL) {
- CarModel->carDesc != NULL;
- free(CarModel->carDesc);
+ if (CarModel->carDesc != NULL)
+ free(CarModel->carDesc);
+ if (CarModel->carXY != NULL)
+ free(CarModel->carXY);
free(CarModel);
CarModel = NULL;
}
- CarModel = (car_model *)malloc(sizeof(CarModel));
+ CarModel = (car_model *)malloc(sizeof(car_model));
CarModel->basePoint.X = basePoint[0];
CarModel->basePoint.Y = basePoint[1];
CarModel->axial[0] = axial[0];
@@ -365,6 +218,7 @@
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]) +
@@ -386,6 +240,8 @@
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);
}
DEBUG("SetCarMeasurePoint Calc Over");
@@ -412,224 +268,161 @@
MA_SendExamStatus(1, 0);
}
-void UpdateRTKInfo(rtk_info *s)
+void UpdateRTKInfo(const 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++;
+ if (s->qf > 0) {
+ RtkBuffer[RtkBufferIn] = *s;
+ RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE;
+ if (RtkBufferNum < RTK_BUFFER_SIZE)
+ RtkBufferNum++;
}
- pthread_mutex_unlock(&add_rtk_history_mutex);
-// DEBUG("UpdateRTKInfo qf = %d tm %ld", s->qf, mktime(&test_tm) - tz.tz_minuteswest*60);
+ 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 = -1;
+ brief.move = move;
+ brief.speed = speed;
+ 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.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);
+ }
}
-void UpdateCarCoord(void)
+/*************************************************
+ * 2娆￠噰鏍风浉宸殑鏃堕棿
+ * @param a
+ * @param b
+ * @return ms
+ */
+static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b)
{
- double azimuth;
- PointF coord;
+ return ABS(TimeMakeComposite(2000 + a->YY, a->MM, a->DD, a->hh, a->mm, a->ss) * 1000 + a->dss*10 -
+ (TimeMakeComposite(2000 + b->YY, b->MM, b->DD, b->hh, b->mm, b->ss) * 1000 + b->dss*10));
+}
- // 璁＄畻鏈�杩戠殑鍑犱釜锛屾粦鍔ㄥ钩鍧�
- int cnt = 0, valid_cnt = 0;
- int s, sm;
- time_t prev_time = 0;
- time_t last_rtk_time = 0;
+static bool UpdateCarCoord(double &spd, int &mov, int &idx)
+{
+ if (CarModel == NULL)
+ return false;
- pthread_mutex_lock(&add_rtk_history_mutex);
- s = rtk_history_in;
- sm = rtk_history_num;
- pthread_mutex_unlock(&add_rtk_history_mutex);
+ if (RtkBufferNum < 3)
+ return false;
- DEBUG("UpdateCarCoord rtk_history_in %d rtk_history_num %d car_coord_num %d", s, sm, car_coord_num);
+ int p1 = ((RtkBufferIn-1)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
+ int p2 = ((RtkBufferIn-2)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
+ int p3 = ((RtkBufferIn-3)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
- // 濡傛灉鍑虹幇QF涓嶆槸鍥哄畾瑙ｃ�丟PS鎶ユ枃涓㈠け鐨勬儏鍐碉紝灏辨寜涓婃鐨勮椹剁姸鎬佸仛鐩寸嚎琛岄┒
- for (; cnt < sm && valid_cnt < MOV_AVG_SIZE; cnt++) {
- if (s == 0) {
- s = RTK_HISTORY_SIZE - 1;
- } else {
- s--;
- }
+ // 濡傛灉涓�瀹氱殑鏃堕棿閮芥病鏈夋湁鏁堝畾浣嶏紝鍒犻櫎涔嬪墠鐨勫��
+ uint32_t tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2]);
- 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 (tmDiff > D_SEC(5)) {
+ if (p1 != 0)
+ RtkBuffer[0] = RtkBuffer[p1];
+ RtkBufferIn = RtkBufferNum = 1;
+ return false;
}
- if (valid_cnt >= MOV_AVG_SIZE) {
- azimuth /= MOV_AVG_SIZE;
- coord.X /= MOV_AVG_SIZE;
- coord.Y /= MOV_AVG_SIZE;
+ // 璁＄畻杞﹁締杞粨鐐�
+ PointF main_ant_coord;
+ main_ant_coord.X = RtkBuffer[p1].x;
+ main_ant_coord.Y = RtkBuffer[p1].y;
- CarCoord[car_coord_in].uptime = AppTimer_GetTickCount();
+ UpdateCarBodyCoord(RtkBuffer[p1].heading, main_ant_coord, CarModel);
- 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;
+ // 璁＄畻閫熷害(绫�/绉�)銆佸墠杩涘悗閫�
+ 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;
- 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++;
+ if (speed < 0.05) {
+ // 鍋滆溅
+ move = 0;
} 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;
+ // 鍒ゆ柇鍓嶈繘杩樻槸鍚庨��
+ 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 {
- // 鍒ゆ柇鍓嶈繘杩樻槸鍚庨��
- double deg = 0.0;
+ deg = atan(fabs(RtkBuffer[p1].x - RtkBuffer[p2].x) /
+ fabs(RtkBuffer[p1].y - RtkBuffer[p2].y));
- 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;
- }
+ deg = toDegree(deg);
- 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));
+ if (RtkBuffer[p1].x > RtkBuffer[p2].x &&
+ RtkBuffer[p1].y > RtkBuffer[p2].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) {
+ } 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;
}
- 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);
+ deg = fabs(RtkBuffer[p1].heading - deg);
+ if (deg > 180) {
+ deg = 360 - deg;
+ }
+ if (deg < 90) {
+ // 鍓嶈繘
+ move = 1;
+ } else {
+ // 鍚庨��
+ move = -1;
+ }
+ }
-// 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);
-// }
+ spd = speed;
+ mov = move;
+ idx = p1;
+ DEBUG("speed = %f m/Sec move = %d", speed, move);
-
- 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;
+ return true;
+/* if (!TestStart) return;
if (CarInArea == 0) {
// if (FrontTireEnterArea(&py, &theParkEdgeMap)) {
@@ -709,16 +502,11 @@
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;
}
@@ -727,51 +515,17 @@
* @param azimuth
* @param coord
*/
-static void UpdateCarBodyCoord(double azimuth, PointF coord, car_model_cache_t *carModel, int &in, int &num)
+static void UpdateCarBodyCoord(double azimuth, PointF main_ant, const car_model *carModel)
{
- carModel[in].uptime = AppTimer_GetTickCount();
- carModel[in].desc = carModelDescFile;
- carModel[in].point_num = 6;
+ for (int i = 0; i < carModel->pointNum; ++i) {
+ double tx = main_ant.X + carModel->carDesc[i].distance*sin(toRadians(azimuth));
+ double ty = main_ant.Y + carModel->carDesc[i].distance*cos(toRadians(azimuth));
- if (carModel[in].points == NULL) {
- carModel[in].points = (PointF *) malloc(sizeof(PointF) * carModel[in].point_num);
+ carModel->carXY[i].X = (tx-main_ant.X)*cos(toRadians(carModel->carDesc[i].angle)) -
+ (ty-main_ant.Y)*sin(toRadians(carModel->carDesc[i].angle)) + main_ant.X;
+ carModel->carXY[i].Y = (tx-main_ant.X)*sin(toRadians(carModel->carDesc[i].angle)) +
+ (ty-main_ant.Y)*cos(toRadians(carModel->carDesc[i].angle)) + main_ant.Y;
}
-
- 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)
--
Gitblit v1.8.0