From 8a09b209f1c546a2fa15329e8f69b4a4f89557c9 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 21 八月 2020 18:59:19 +0800
Subject: [PATCH] 自适应触发和个子项修改。
---
lib/src/main/cpp/test_items2/drive_straight.cpp | 173 +++++++++++++--------------------------------------------
1 files changed, 41 insertions(+), 132 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/drive_straight.cpp b/lib/src/main/cpp/test_items2/drive_straight.cpp
index 3f26008..8c69204 100644
--- a/lib/src/main/cpp/test_items2/drive_straight.cpp
+++ b/lib/src/main/cpp/test_items2/drive_straight.cpp
@@ -9,6 +9,7 @@
#include "road_exam.h"
#include "../master/comm_if.h"
#include "../common/apptimer.h"
+#include "../test_common/odo_graph.h"
#include <cmath>
#define DEBUG(fmt, args...) LOGD("<drive_straight> <%s>: " fmt, __func__, ##args)
@@ -16,148 +17,56 @@
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 int examTtsSeq;
+static int setup;
+static double beginOdo;
+static int yaw_stat;
-static double CalcDistance2Edge(road_t &road, const car_model *car);
-static void PlayTTSTimeout(union sigval sig);
+static void TtsBack(int seq)
+{
+ setup = 1;
+}
-void StartDriveStraightExam(std::string tts) {
+void StartDriveStraightExam(void) {
DEBUG("寮�濮嬬洿绾胯椹�");
-
- ttsPlayEnd = 0;
- if (!tts.empty()) {
- examTtsSeq = PlayTTS(tts.c_str(), NULL);
- } else {
- examTtsSeq = PlayTTS("璇蜂繚鎸佺洿绾胯椹�", NULL);
- }
-
- distanceToStartSum = 0;
- reportOffsetOver = false;
-
- AppTimer_delete(PlayTTSTimeout);
- AppTimer_add(PlayTTSTimeout, D_SEC(5));
+ setup = 0;
+ yaw_stat = 0;
+ PlayTTS("璇蜂繚鎸佺洿绾胯椹�", TtsBack);
}
-int ExecuteDriveStraightExam(road_t &road, const car_model *car,
- const trigger_line_t *item, const struct RtkTime *rtkTime) {
- static PointF startPoint;
+bool TestDriveStraight(const car_model *car, const struct RtkTime *rtkTime) {
+ static double yaw;
+ static Line baseLine;
- double dis2roadEdge = 0;
+ if (setup == 1) {
+ // 鍋忚埅瑙掑钩鍧囧��
+ if (yaw_stat == 0) {
+ yaw = car->yaw;
+ } else {
+ yaw += car->yaw;
+ }
+ yaw_stat++;
+ if (yaw_stat == 5) {
+ yaw = fmod(yaw, 360) / 5;
- if (ttsPlayEnd == 1) {
- ttsPlayEnd = 2;
- startPoint = car->basePoint;
- edgeDistance = CalcDistance2Edge(road, car); // 鍩哄噯杈硅窛
-
- DEBUG("褰撳墠鍩哄噯璺竟闂磋窛 %f", edgeDistance);
+ PointF extPoint = PointExtend(car->basePoint, 100, yaw);
+ MakeLine(&baseLine, &car->basePoint, &extPoint);
+ beginOdo = ReadOdo();
+ setup = 2;
+ }
+ } else if (setup == 2) {
+ if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine) > MAX_OFFSET_DISTANCE) {
+ DEBUG("鐩寸嚎鍋忕Щ澶т簬30鍘樼背");
+ // 鍋忕Щ澶т簬30鍘樼背锛屼笉鍚堟牸
+ AddExamFault(30, rtkTime);
+ return false;
+ }
}
- 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 > 105) {
- DEBUG("绂诲紑鐩寸嚎琛岄┒鍖哄煙");
+ if (setup == 2 && ReadOdo() - beginOdo > CHECK_STAGE_DISTANCE) {
+ DEBUG("鐩寸嚎琛岄┒缁撴潫");
PlayTTS("鐩寸嚎琛岄┒缁撴潫", NULL);
- return -1;
- }
- return 1;
-}
-
-void DriveStraightTTSDone(int id)
-{
- // 绛夎闊虫挱鎶ュ畬姣曞悗璁℃椂
- if (id == examTtsSeq) {
- DEBUG("StopCarTTSDone %d", id);
- ttsPlayEnd = 1;
- AppTimer_delete(PlayTTSTimeout);
- }
-}
-
-void TerminateDriveStraightExam(void)
-{
- AppTimer_delete(PlayTTSTimeout);
-}
-
-static void PlayTTSTimeout(union sigval sig)
-{
- AppTimer_delete(PlayTTSTimeout);
-
- ttsPlayEnd = 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;
- }
+ return false;
}
-FIND_VP_END:
- if (get_vp) {
- DEBUG("寰楀埌鍨傜偣 %d: %f, %f -- %f, %f", road.id, car->basePoint.X, car->basePoint.Y, vp.X, vp.Y);
- distance = DistanceOf(car->basePoint, vp);
- } else {
- // 娌℃湁鎵惧埌鍖归厤绾跨锛屾寜鏈�灏忚窛绂婚《鐐硅绠�
- DEBUG("鏃犲瀭鐐�");
- 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;
+ return true;
}
--
Gitblit v1.8.0