From 27fc91fbe8f88b6885356e68828cfe1ce1db7601 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期三, 28 二月 2024 09:53:13 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_common/car_sensor.h | 41
lib/src/main/java/com/anyun/exam/lib/IRtcmData.kt | 5
lib/src/main/java/com/anyun/exam/lib/net/MessageHandler.kt | 80
lib/src/main/cpp/CMakeLists.txt | 8
lib/src/main/java/com/anyun/exam/lib/net/ExceptionHandler.kt | 24
lib/src/main/cpp/test_items/park_bottom.cpp | 423 +--
lib/src/main/java/com/anyun/exam/lib/ActionRtcm.kt | 7
lib/src/main/java/com/anyun/exam/lib/ActionManager.kt | 39
lib/src/main/cpp/native-lib.cpp | 25
lib/src/main/java/com/anyun/exam/lib/net/NtripEncoder.kt | 15
lib/src/main/cpp/teach/teach.h | 14
lib/src/main/cpp/rtk_module/parse_gps.cpp | 8
lib/src/main/cpp/teach/train.h | 12
lib/src/main/java/com/anyun/exam/lib/net/ConnectorIdleStateTrigger.kt | 32
lib/src/main/cpp/mcu/car_box.h | 13
lib/src/main/cpp/test_items/right_corner.cpp | 137 +
lib/src/main/cpp/driver_test.h | 205 -
lib/src/main/java/com/anyun/exam/lib/net/Constant.kt | 8
lib/src/main/java/com/anyun/exam/lib/util/ByteUtil.java | 15
lib/src/main/cpp/rtcm.h | 12
lib/src/main/java/com/anyun/exam/lib/net/NtripDecoder.kt | 22
lib/src/main/cpp/mcu/car_box.cpp | 112 +
lib/src/main/cpp/rtk_module/parse_gps.h | 2
lib/src/main/cpp/test_items/area_exam.h | 9
lib/src/main/java/com/anyun/exam/lib/BaseAction.kt | 5
lib/src/main/cpp/test_items/uphill.h | 14
lib/src/main/cpp/rtk_module/virtual_rtk.cpp | 19
lib/src/main/cpp/driver_test.cpp | 567 +++--
lib/src/main/cpp/rtk_module/rtk.cpp | 6
lib/src/main/cpp/test_common/Geometry.cpp | 174
lib/src/main/cpp/test_items/driving_curve.h | 7
lib/src/main/cpp/test_items/uphill.cpp | 75
lib/src/main/java/com/anyun/exam/lib/net/NettyTcpClient.kt | 134 +
lib/src/main/cpp/test_items/park_edge.cpp | 519 +----
lib/src/main/cpp/rapidjson/reader.h | 4
lib/src/main/cpp/master/comm_if.cpp | 822 +-------
lib/src/main/java/com/anyun/exam/lib/Rtcm.kt | 17
lib/src/main/java/com/anyun/exam/lib/net/ConnectionWatchdog.kt | 42
lib/src/main/java/com/anyun/exam/lib/net/ParseNtripPackage.kt | 147 +
lib/src/main/cpp/test_common/Geometry.h | 85
lib/src/main/cpp/rtk_platform/platform.cpp | 13
lib/src/main/cpp/map.h | 16
lib/src/main/cpp/test_items/driving_curve.cpp | 383 ---
lib/src/main/cpp/test_items/right_corner.h | 15
lib/src/main/cpp/test_items/park_edge.h | 8
lib/src/main/cpp/teach/train.cpp | 51
lib/src/main/cpp/test_items/park_bottom.h | 4
lib/src/main/cpp/rtcm.cpp | 145 +
lib/src/main/cpp/test_items/area_exam.cpp | 463 +++-
lib/src/main/cpp/teach/teach.cpp | 209 ++
lib/src/main/cpp/master/comm_if.h | 2
lib/src/main/cpp/common/apptimer.cpp | 2
/dev/null | 16
lib/src/main/java/com/anyun/exam/lib/net/TcpSession.kt | 13
lib/src/main/cpp/test_common/car_sensor.cpp | 132
lib/src/main/java/com/anyun/exam/lib/RemoteService.java | 25
lib/src/main/java/com/anyun/exam/lib/net/NtripTcpClient.kt | 26
57 files changed, 2,890 insertions(+), 2,538 deletions(-)
diff --git a/lib/src/main/cpp/CMakeLists.txt b/lib/src/main/cpp/CMakeLists.txt
index 32b4e42..2f9a473 100644
--- a/lib/src/main/cpp/CMakeLists.txt
+++ b/lib/src/main/cpp/CMakeLists.txt
@@ -21,6 +21,7 @@
# Provides a relative path to your source file(s).
native-lib.cpp
+ rtcm.cpp
common/serial_port.cpp
common/net.cpp
common/apptimer.cpp
@@ -37,16 +38,19 @@
mcu/ahp.cpp
mcu/ada.cpp
mcu/dfu.cpp
+ mcu/car_box.cpp
test_common/car_sensor.cpp
test_common/odo_graph.cpp
test_items/error_list.cpp
test_items/park_edge.cpp
test_items/park_bottom.cpp
- test_items/stop_and_start.cpp
+ test_items/uphill.cpp
test_items/driving_curve.cpp
- test_items/turn_a90.cpp
+ test_items/right_corner.cpp
test_items/area_exam.cpp
+ teach/teach.cpp
+ teach/train.cpp
rtk_module/rtk.cpp
rtk_module/virtual_rtk.cpp
diff --git a/lib/src/main/cpp/common/apptimer.cpp b/lib/src/main/cpp/common/apptimer.cpp
index ff87408..9bec9b0 100644
--- a/lib/src/main/cpp/common/apptimer.cpp
+++ b/lib/src/main/cpp/common/apptimer.cpp
@@ -61,7 +61,7 @@
callback(timer->var);
}
} else {
-// LOGD("Cancel %d", static_cast<int>(timer->flag));
+// LOGD("Cancel %d", static_cast<int>(timer->stopFlag));
RemoveTimerList(timer);
}
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 1563f56..a85c306 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -15,6 +15,7 @@
#include <numeric>
#include <algorithm>
#include <string>
+#include <tuple>
#include "driver_test.h"
#include "defs.h"
@@ -26,9 +27,9 @@
#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/right_corner.h"
#include "test_items/driving_curve.h"
-#include "test_items/stop_and_start.h"
+#include "test_items/uphill.h"
#include "master/comm_if.h"
#include "utils/xconvert.h"
#include "utils/num.h"
@@ -127,19 +128,13 @@
static const char DEFAULT_CROSSING_TURN_BACK_TTS[] = "鍓嶆柟閫夋嫨鍚堥�傚湴鐐规帀澶�";
static const char DEFAULT_CROSSING_TURN_UNKNOWN_TTS[] = "鍓嶆柟璺彛鍚粠鏁欑粌鎸囩ず";
-static bool ExamStart = false;
static int ExamType;
static bool reportSeatbeltEject;
vector<ExamFault> ExamFaultList;
static int examFaultIndex = 0;
-//static LIST_AREA_MAP AreaMapList;
-static area_map_t AreaMap;
-
ilovers::semaphore sem(0);
-
-static road_exam_map RoadMap;
static int exam_dummy_light;
@@ -152,8 +147,6 @@
static bool engineRuning = false;
const int ENGINE_MIN_ROTATE = 200;
static bool engineStart = false;
-
-exam_param_t examParam;
#define MOV_AVG_SIZE 1
#define RTK_BUFFER_SIZE 100
@@ -177,19 +170,23 @@
static void work_thread(void);
static void UploadModeling(motion_t &motion, modeling_t &modeling);
+prime_t& GetPrime(void)
+{
+ return prime;
+}
+
void DriverTestInit(void)
{
- ExamStart = false;
+ prime.examing = false;
+ prime.pMotion = &realtimeMotionStatus;
+ prime.pModeling = realtimeBodyModeling;
+ prime.pModel = &CarModel;
+
SetExamParamDefault();
// CarModelList.clear();
// AreaMapList.clear();
-
- RoadMap.roads.clear();
- RoadMap.specialAreas.clear();
- RoadMap.forbidLines.clear();
- RoadMap.examScheme.clear();
CarSensorInit();
@@ -201,97 +198,97 @@
static void SetExamParamDefault(void)
{
- examParam.hold_start_key_limit_time = DEFAULT_START_KEY_HOLD_TIME;
- examParam.curve_pause_criteria = DEFAULT_CURVE_PAUSE_TIME;
- examParam.park_bottom_pause_criteria = DEFAULT_PARK_BOTTOM_PAUSE_TIME;
- examParam.park_bottom_limit_time = DEFAULT_PARK_BOTTOM_FINISH_TIME;
- examParam.park_edge_pause_criteria = DEFAULT_PART_EDGE_PAUSE_TIME;
- examParam.park_edge_limit_time = DEFAULT_PARK_EDGE_FINISH_TIME;
- examParam.turn_a90_pause_criteria = DEFAULT_TURN_A90_PAUSE_TIME;
+ prime.examParam.hold_start_key_limit_time = DEFAULT_START_KEY_HOLD_TIME;
+ prime.examParam.curve_pause_criteria = DEFAULT_CURVE_PAUSE_TIME;
+ prime.examParam.park_bottom_pause_criteria = DEFAULT_PARK_BOTTOM_PAUSE_TIME;
+ prime.examParam.park_bottom_limit_time = DEFAULT_PARK_BOTTOM_FINISH_TIME;
+ prime.examParam.park_edge_pause_criteria = DEFAULT_PART_EDGE_PAUSE_TIME;
+ prime.examParam.park_edge_limit_time = DEFAULT_PARK_EDGE_FINISH_TIME;
+ prime.examParam.turn_a90_pause_criteria = DEFAULT_TURN_A90_PAUSE_TIME;
- examParam.ramp_stoppoint_red_distance = DEFAULT_RAMP_STOPPOINT_RED_DISTANCE;
- examParam.ramp_edge_yellow_distance = DEFAULT_RAMP_EDGE_YELLOW_DISTANCE;
- examParam.ramp_edge_red_distance = DEFAULT_RAMP_EDGE_RED_DISTANCE;
- examParam.ramp_slide_yellow_distance = DEFAULT_RAMP_SLIDE_YELLOW_DISTANCE;
- examParam.ramp_slide_red_distance = DEFAULT_RAMP_SLIDE_RED_DISTANCE;
- examParam.ramp_start_car_limit_time = DEFAULT_RAMP_FINISH_TIME;
+ prime.examParam.ramp_stoppoint_red_distance = DEFAULT_RAMP_STOPPOINT_RED_DISTANCE;
+ prime.examParam.ramp_edge_yellow_distance = DEFAULT_RAMP_EDGE_YELLOW_DISTANCE;
+ prime.examParam.ramp_edge_red_distance = DEFAULT_RAMP_EDGE_RED_DISTANCE;
+ prime.examParam.ramp_slide_yellow_distance = DEFAULT_RAMP_SLIDE_YELLOW_DISTANCE;
+ prime.examParam.ramp_slide_red_distance = DEFAULT_RAMP_SLIDE_RED_DISTANCE;
+ prime.examParam.ramp_start_car_limit_time = DEFAULT_RAMP_FINISH_TIME;
- examParam.road_slide_yellow_distance = DEFAULT_ROAD_SLIDE_YELLOW_DISTANCE;
- examParam.road_slide_red_distance = DEFAULT_ROAD_SLIDE_RED_DISTANCE;
- examParam.road_total_distance = DEFAULT_ROAD_MAX_DISTANCE;
- examParam.road_max_speed = DEFAULT_ROAD_MAX_SPEED;
+ prime.examParam.road_slide_yellow_distance = DEFAULT_ROAD_SLIDE_YELLOW_DISTANCE;
+ prime.examParam.road_slide_red_distance = DEFAULT_ROAD_SLIDE_RED_DISTANCE;
+ prime.examParam.road_total_distance = DEFAULT_ROAD_MAX_DISTANCE;
+ prime.examParam.road_max_speed = DEFAULT_ROAD_MAX_SPEED;
- examParam.gear_speed_table.resize(6);
- for (int i = 0; i < examParam.gear_speed_table.size(); ++i) {
- examParam.gear_speed_table[i].resize(2);
+ prime.examParam.gear_speed_table.resize(6);
+ for (int i = 0; i < prime.examParam.gear_speed_table.size(); ++i) {
+ prime.examParam.gear_speed_table[i].resize(2);
}
- examParam.gear_speed_table[0][0] = 0;
- examParam.gear_speed_table[0][1] = 20;
- examParam.gear_speed_table[1][0] = 5;
- examParam.gear_speed_table[1][1] = 30;
- examParam.gear_speed_table[2][0] = 15;
- examParam.gear_speed_table[2][1] = 40;
- examParam.gear_speed_table[3][0] = 25;
- examParam.gear_speed_table[3][1] = 10000;
- examParam.gear_speed_table[4][0] = 35;
- examParam.gear_speed_table[4][1] = 10000;
- examParam.gear_speed_table[5][0] = 45;
- examParam.gear_speed_table[5][1] = 10000;
+ prime.examParam.gear_speed_table[0][0] = 0;
+ prime.examParam.gear_speed_table[0][1] = 20;
+ prime.examParam.gear_speed_table[1][0] = 5;
+ prime.examParam.gear_speed_table[1][1] = 30;
+ prime.examParam.gear_speed_table[2][0] = 15;
+ prime.examParam.gear_speed_table[2][1] = 40;
+ prime.examParam.gear_speed_table[3][0] = 25;
+ prime.examParam.gear_speed_table[3][1] = 10000;
+ prime.examParam.gear_speed_table[4][0] = 35;
+ prime.examParam.gear_speed_table[4][1] = 10000;
+ prime.examParam.gear_speed_table[5][0] = 45;
+ prime.examParam.gear_speed_table[5][1] = 10000;
- examParam.gear_n_allow_time = DEFAULT_GEAR_N_TIME;
- examParam.same_gear_min_time = DEFAULT_SAME_GEAR_HOLD_TIME; // x绉掑唴锛屼笉鍏佽N->X->N->X缃悓涓�鎸′綅
- examParam.gear_speed_error_cumulative_time = DEFAULT_GEAR_SPEED_ERROR_MAX_TIME;
- examParam.road_pause_criteria = DEFAULT_ROAD_PAUSE_TIME;
- examParam.continuous_change_lane_min_time = DEFAULT_CHANGE_LANE_MIN_TIME;
- examParam.crash_dotted_line_cumulative_time = DEFAULT_CRASH_DOTTED_LINE_MAX_TIME;
- examParam.turn_signal_min_advance = DEFAULT_TURN_SIGNAL_MIN_ADVANCE;
- examParam.start_car_max_rpm = DEFAULT_START_CAR_MAX_RMP;
- examParam.start_car_limit_distance = DEFAULT_START_CAR_DISTANCE;
- examParam.open_door_drive_allow_distance = DEFAULT_START_CAR_OPEN_DOOR_DISTANCE;
+ prime.examParam.gear_n_allow_time = DEFAULT_GEAR_N_TIME;
+ prime.examParam.same_gear_min_time = DEFAULT_SAME_GEAR_HOLD_TIME; // x绉掑唴锛屼笉鍏佽N->X->N->X缃悓涓�鎸′綅
+ prime.examParam.gear_speed_error_cumulative_time = DEFAULT_GEAR_SPEED_ERROR_MAX_TIME;
+ prime.examParam.road_pause_criteria = DEFAULT_ROAD_PAUSE_TIME;
+ prime.examParam.continuous_change_lane_min_time = DEFAULT_CHANGE_LANE_MIN_TIME;
+ prime.examParam.crash_dotted_line_cumulative_time = DEFAULT_CRASH_DOTTED_LINE_MAX_TIME;
+ prime.examParam.turn_signal_min_advance = DEFAULT_TURN_SIGNAL_MIN_ADVANCE;
+ prime.examParam.start_car_max_rpm = DEFAULT_START_CAR_MAX_RMP;
+ prime.examParam.start_car_limit_distance = DEFAULT_START_CAR_DISTANCE;
+ prime.examParam.open_door_drive_allow_distance = DEFAULT_START_CAR_OPEN_DOOR_DISTANCE;
- examParam.prepare_tts = DEFAULT_PREPARE_TTS;
- examParam.touch_leftfront_tts = DEFAULT_TOUCH_LEFT_FRONT;
- examParam.touch_leftrear_tts = DEFAULT_TOUCH_LEFT_REAR;
- examParam.touch_rightfront_tts = DEFAULT_TOUCH_RIGHT_FRONT;
- examParam.touch_rightrear_tts = DEFAULT_TOUCH_RIGHT_REAR;
- examParam.start_engine_tts = DEFAULT_START_ENGINE;
+ prime.examParam.prepare_tts = DEFAULT_PREPARE_TTS;
+ prime.examParam.touch_leftfront_tts = DEFAULT_TOUCH_LEFT_FRONT;
+ prime.examParam.touch_leftrear_tts = DEFAULT_TOUCH_LEFT_REAR;
+ prime.examParam.touch_rightfront_tts = DEFAULT_TOUCH_RIGHT_FRONT;
+ prime.examParam.touch_rightrear_tts = DEFAULT_TOUCH_RIGHT_REAR;
+ prime.examParam.start_engine_tts = DEFAULT_START_ENGINE;
- examParam.start_car_begin_tts = DEFAULT_START_CAR_BEGIN_TTS;
- examParam.start_car_end_tts = DEFAULT_START_CAR_END_TTS;
- examParam.change_lane_limit_distance = CHANGE_LANE_MAX_DISTANCE;
- examParam.change_lane_begin_tts = DEFAULT_CHANGE_LANE_BEGIN_TTS;
- examParam.change_lane_end_tts = DEFAULT_CHANGE_LANE_END_TTS;
- examParam.shift_limit_distance = DEFAULT_SHIFT_MAX_DISTANCE;
- examParam.shift_hold_time = DEFAULT_SHIFT_HOLD_TIME;
- examParam.shift_begin_tts = DEFAULT_SHIFT_BEGIN_TTS;
- examParam.shift_end_tts = DEFAULT_SHIFT_END_TTS;
- examParam.shift_up_tts = "";
- examParam.shift_down_tts = "";
+ prime.examParam.start_car_begin_tts = DEFAULT_START_CAR_BEGIN_TTS;
+ prime.examParam.start_car_end_tts = DEFAULT_START_CAR_END_TTS;
+ prime.examParam.change_lane_limit_distance = CHANGE_LANE_MAX_DISTANCE;
+ prime.examParam.change_lane_begin_tts = DEFAULT_CHANGE_LANE_BEGIN_TTS;
+ prime.examParam.change_lane_end_tts = DEFAULT_CHANGE_LANE_END_TTS;
+ prime.examParam.shift_limit_distance = DEFAULT_SHIFT_MAX_DISTANCE;
+ prime.examParam.shift_hold_time = DEFAULT_SHIFT_HOLD_TIME;
+ prime.examParam.shift_begin_tts = DEFAULT_SHIFT_BEGIN_TTS;
+ prime.examParam.shift_end_tts = DEFAULT_SHIFT_END_TTS;
+ prime.examParam.shift_up_tts = "";
+ prime.examParam.shift_down_tts = "";
- examParam.straight_begin_tts = DEFAULT_STRAIGHT_BEGIN_TTS;
- examParam.straight_end_tts = DEFAULT_STRAIGHT_END_TTS;
- examParam.straight_limit_distance = DEFAULT_STRAIGHT_MAX_DISTANCE;
- examParam.straight_max_offset = DEFAULT_STRAIGHT_MAX_OFFSET;
+ prime.examParam.straight_begin_tts = DEFAULT_STRAIGHT_BEGIN_TTS;
+ prime.examParam.straight_end_tts = DEFAULT_STRAIGHT_END_TTS;
+ prime.examParam.straight_limit_distance = DEFAULT_STRAIGHT_MAX_DISTANCE;
+ prime.examParam.straight_max_offset = DEFAULT_STRAIGHT_MAX_OFFSET;
- examParam.overtake_limit_distance = DEFAULT_OVERTAKE_MAX_DISTANCE;
- examParam.overtake_begin_tts = DEFAULT_OVERTAKE_BEGIN_TTS;
- examParam.overtake_end_tts = DEFAULT_OVERTAKE_END_TTS;
+ prime.examParam.overtake_limit_distance = DEFAULT_OVERTAKE_MAX_DISTANCE;
+ prime.examParam.overtake_begin_tts = DEFAULT_OVERTAKE_BEGIN_TTS;
+ prime.examParam.overtake_end_tts = DEFAULT_OVERTAKE_END_TTS;
- examParam.stop_car_limit_distance = DEFAULT_STOP_CAR_MAX_DISTANCE;
- examParam.stop_car_open_door_allow_time = DEFAULT_STOP_CAR_OPEN_DOOR_MAX_TIME;
- examParam.stop_car_edge_red_distance = DEFAULT_STOP_CAR_EDGE_RED_DISTANCE;
- examParam.stop_car_edge_yellow_distance = DEFAULT_STOP_CAR_EDGE_YELLOW_DISTANCE;
- examParam.stop_car_begin_tts = DEFAULT_STOP_CAR_BEGIN_TTS;
- examParam.stop_car_end_tts = DEFAULT_STOP_CAR_END_TTS;
- examParam.crossing_stop_valid_distance = DEFAULT_CROSSING_STOP_VALID_DISTANCE;
- examParam.cross_school_max_speed = DEFAULT_CROSS_SCHOOL_MAX_SPEED;
- examParam.crossing_break_valid_distance = DEFAULT_CROSS_BREAK_VALID_DISTANCE;
+ prime.examParam.stop_car_limit_distance = DEFAULT_STOP_CAR_MAX_DISTANCE;
+ prime.examParam.stop_car_open_door_allow_time = DEFAULT_STOP_CAR_OPEN_DOOR_MAX_TIME;
+ prime.examParam.stop_car_edge_red_distance = DEFAULT_STOP_CAR_EDGE_RED_DISTANCE;
+ prime.examParam.stop_car_edge_yellow_distance = DEFAULT_STOP_CAR_EDGE_YELLOW_DISTANCE;
+ prime.examParam.stop_car_begin_tts = DEFAULT_STOP_CAR_BEGIN_TTS;
+ prime.examParam.stop_car_end_tts = DEFAULT_STOP_CAR_END_TTS;
+ prime.examParam.crossing_stop_valid_distance = DEFAULT_CROSSING_STOP_VALID_DISTANCE;
+ prime.examParam.cross_school_max_speed = DEFAULT_CROSS_SCHOOL_MAX_SPEED;
+ prime.examParam.crossing_break_valid_distance = DEFAULT_CROSS_BREAK_VALID_DISTANCE;
- examParam.crossing_go_straight_tts = DEFAULT_CROSSING_GO_STRAIGHT_TTS;
- examParam.crossing_turn_left_tts = DEFAULT_CROSSING_TURN_LEFT_TTS;
- examParam.crossing_turn_right_tts = DEFAULT_CROSSING_TURN_RIGHT_TTS;
- examParam.crossing_turn_back_tts = DEFAULT_CROSSING_TURN_BACK_TTS;
- examParam.crossing_turn_unknown_tts = DEFAULT_CROSSING_TURN_UNKNOWN_TTS;
+ prime.examParam.crossing_go_straight_tts = DEFAULT_CROSSING_GO_STRAIGHT_TTS;
+ prime.examParam.crossing_turn_left_tts = DEFAULT_CROSSING_TURN_LEFT_TTS;
+ prime.examParam.crossing_turn_right_tts = DEFAULT_CROSSING_TURN_RIGHT_TTS;
+ prime.examParam.crossing_turn_back_tts = DEFAULT_CROSSING_TURN_BACK_TTS;
+ prime.examParam.crossing_turn_unknown_tts = DEFAULT_CROSSING_TURN_UNKNOWN_TTS;
}
static void ReadDriverExamPrimerTimeout(apptimer_var_t val)
@@ -308,134 +305,113 @@
void ClearAreaMap(void)
{
- if (ExamStart)
- return;
-
-// for (int i = 0; i < AreaMapList.size(); ++i) {
-// if (AreaMapList[i].map.point != NULL)
-// free(AreaMapList[i].map.point);
-// if (AreaMapList[i].map2.point != NULL)
-// free(AreaMapList[i].map2.point);
-// }
-//
-// LIST_AREA_MAP().swap(AreaMapList);
-
-
- vector<curve_map_t>().swap(AreaMap.curve_map);
- vector<park_button_map_t>().swap(AreaMap.park_button_map);
- vector<park_edge_map_t>().swap(AreaMap.park_edge_map);
- vector<turn_a90_map_t>().swap(AreaMap.turn_a90_map);
- vector<uphill_map_t>().swap(AreaMap.uphill_map);
-}
-
-void AddAreaMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2)
-{
- if (map == NULL || pointNum == 0 || ExamStart)
- return;
- DEBUG("鍔犲叆鍦板浘淇℃伅 id %d type %d pointNum %d point2Num %d", id, type, pointNum, pointNum2);
-
-// struct area_exam_map newMap;
-//
-// newMap.id = id;
-// newMap.type = type;
-// newMap.map.num = pointNum;
-// newMap.map2.num = 0;
-// newMap.map.point = NULL;
-// newMap.map2.point = NULL;
-//
-// if (pointNum > 0) {
-// newMap.map.point = (PointF *) malloc(pointNum * sizeof(PointF));
-// for (int i = 0; i < pointNum; ++i) {
-// newMap.map.point[i].X = map[i][0];
-// newMap.map.point[i].Y = map[i][1];
-// }
-// }
-//
-// if (pointNum2 > 0 && map2 != NULL) {
-// newMap.map2.num = pointNum2;
-// newMap.map2.point = (PointF *) malloc(pointNum2 * sizeof(PointF));
-// for (int i = 0; i < pointNum2; ++i) {
-// newMap.map2.point[i].X = map2[i][0];
-// newMap.map2.point[i].Y = map2[i][1];
-// }
-// }
-//
-// AreaMapList.push_back(newMap);
+ if (!prime.examing) {
+ DEBUG("娓呴櫎鎵�鏈夊湴鍥�");
+ vector<curve_map_t>().swap(std::get<MAP_TYPE_CURVE>(prime.maps));
+ vector<park_button_map_t>().swap(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps));
+ vector<park_edge_map_t>().swap(std::get<MAP_TYPE_PARK_EDGE>(prime.maps));
+ vector<turn_a90_map_t>().swap(std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps));
+ vector<uphill_map_t>().swap(std::get<MAP_TYPE_UPHILL>(prime.maps));
+ }
}
void AddCurveMap(curve_map_t &map)
{
- AreaMap.curve_map.push_back(map);
+ DEBUG("娣诲姞鏇茬嚎琛岄┒鍦板浘");
+ if (prime.examing) {
+ return;
+ }
+ for (auto m: std::get<MAP_TYPE_CURVE>(prime.maps)) {
+ if (m.id == map.id) {
+ return;
+ }
+ }
+
+ std::get<MAP_TYPE_CURVE>(prime.maps).push_back(map);
}
+// 濡傛灉鏈噰闆嗚溅浣嶅彛铏氱嚎鍐呬晶锛屽垯闇�瑕佽绠楀嚭鏉�
void AddParkButtonMap(park_button_map_t &map)
{
- AreaMap.park_button_map.push_back(map);
+ DEBUG("娣诲姞鍊掕溅鍏ュ簱鍦板浘");
+ if (prime.examing) {
+ return;
+ }
+
+ for (auto m: std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)) {
+ if (m.id == map.id) {
+ return;
+ }
+ }
+ if (map.points.size() == 8) {
+ PointF p8 = PointExtend(map.points[2], map.line_width, YawOf(map.points[2], map.points[3]));
+ PointF p9 = PointExtend(map.points[5], map.line_width, YawOf(map.points[5], map.points[4]));
+ map.points.push_back(p8);
+ map.points.push_back(p9);
+ }
+ std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).push_back(map);
+ DEBUG("鍊掑簱鍦板浘鐐规暟锛�%d", map.points.size());
+ DEBUG("鍊掑簱鍦板浘鐐规暟 %d",std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[0].points.size());
}
+// 濡傛灉鏈噰闆嗚溅浣嶅彛铏氱嚎鍐呬晶锛屽垯闇�瑕佽绠楀嚭鏉�
void AddParkEdgeMap(park_edge_map_t &map)
{
- AreaMap.park_edge_map.push_back(map);
-}
-
-void CleanRoadMap(void)
-{
- if (ExamStart) return;
-
- DEBUG("娓呴櫎鏃х殑璺�冨湴鍥�");
-
- vector<road_t>().swap(RoadMap.roads);
- vector<special_area_t>().swap(RoadMap.specialAreas);
- vector<forbid_line_t>().swap(RoadMap.forbidLines);
-// vector<scheme_t>().swap(RoadMap.examScheme);
-}
-
-void SetRoadMap(road_exam_map &map, vector<scheme_t> &scheme)
-{
- if (ExamStart) return;
-
- RoadMap.roads.assign(map.roads.begin(), map.roads.end());
- RoadMap.specialAreas.assign(map.specialAreas.begin(), map.specialAreas.end());
-// RoadMap.triggerLines.assign(map.triggerLines.begin(), map.triggerLines.end());
- RoadMap.forbidLines.assign(map.forbidLines.begin(), map.forbidLines.end());
-
-// RoadMap.examScheme.assign(scheme.begin(), scheme.end());
-
- DEBUG("寰楀埌鏂扮殑璺�冨湴鍥� 璺暟閲� %d 鐗规畩鍖哄煙鏁伴噺 %d 鍏朵粬绂佹绾挎暟閲� %d 椤圭洰鏁伴噺 %d", RoadMap.roads.size(), RoadMap.specialAreas.size(), RoadMap.forbidLines.size(), RoadMap.examScheme.size());
-
- /*for (int i = 0; i < RoadMap.roads.size(); ++i) {
- DEBUG("璺� id = %d", RoadMap.roads[i].id);
- DEBUG("宸﹁竟绾挎鏁� %d", RoadMap.roads[i].leftEdge.size());
- for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) {
- int n = RoadMap.roads[i].leftEdge[j].points.size();
-
- DEBUG("\t褰撳墠宸﹁竟绾挎 绫诲瀷 %d 鐐规暟 %d", RoadMap.roads[i].leftEdge[j].character, n);
- for (int k = 0; k < n; ++k) {
- DEBUG("\t\t鐐瑰潗鏍� %d: %f, %f", k, RoadMap.roads[i].leftEdge[j].points[k].X, RoadMap.roads[i].leftEdge[j].points[k].Y);
- }
+ DEBUG("娣诲姞渚ф柟鍋滆溅鍦板浘");
+ if (prime.examing) {
+ return;
+ }
+ for (auto m: std::get<MAP_TYPE_PARK_EDGE>(prime.maps)) {
+ if (m.id == map.id) {
+ return;
}
-
- DEBUG("鍙宠竟绾挎鏁� %d", RoadMap.roads[i].rightEdge.size());
- for (int j = 0; j < RoadMap.roads[i].rightEdge.size(); ++j) {
- int n = RoadMap.roads[i].rightEdge[j].points.size();
-
- DEBUG("\t褰撳墠鍙宠竟绾挎 绫诲瀷 %d 鐐规暟 %d", RoadMap.roads[i].rightEdge[j].character, n);
- for (int k = 0; k < n; ++k) {
- DEBUG("\t\t鐐瑰潗鏍� %d: %f, %f", k, RoadMap.roads[i].rightEdge[j].points[k].X, RoadMap.roads[i].rightEdge[j].points[k].Y);
- }
- }
- }*/
+ }
+ if (map.points.size() == 8) {
+ PointF p8 = PointExtend(map.points[2], map.line_width, YawOf(map.points[2], map.points[3]));
+ PointF p9 = PointExtend(map.points[5], map.line_width, YawOf(map.points[5], map.points[4]));
+ map.points.push_back(p8);
+ map.points.push_back(p9);
+ }
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps).push_back(map);
}
-void SetRoadExamScheme(vector<scheme_t> &scheme)
+// 鍧￠亾璧锋鍦板浘瑕佹妸宸︿笅瑙掑拰宸︿笂瑙掕绠楀嚭鏉ワ紝鏂逛究鍦哄湴鐨勮繘鍏ュ拰閫�鍑哄垽鏂�
+void AddUphillMap(uphill_map_t &map)
{
- if (ExamStart) return;
+ DEBUG("娣诲姞涓婂潯璧锋鍦板浘");
+ if (prime.examing) {
+ return;
+ }
+ for (auto m: std::get<MAP_TYPE_UPHILL>(prime.maps)) {
+ if (m.id == map.id) {
+ return;
+ }
+ }
- vector<scheme_t>().swap(RoadMap.examScheme);
+ if (map.points.size() != 10)
+ return;
- RoadMap.examScheme.assign(scheme.begin(), scheme.end());
+ PointF bottom_left = Calc3Point(map.points[1], map.points[0], DistanceOf(map.points[1], map.points[2]), 'R');
+ PointF top_left = Calc3Point(map.points[8], map.points[9], DistanceOf(map.points[8], map.points[7]), 'L');
- DEBUG("寰楀埌璺�冮」鐩柟妗� 椤圭洰鏁伴噺 %d", RoadMap.examScheme.size());
+ map.points.push_back(bottom_left); // 浣滀负10鍙风偣
+ map.points.push_back(top_left); // 浣滀负11鍙风偣
+
+ std::get<MAP_TYPE_UPHILL>(prime.maps).push_back(map);
+}
+
+void AddTurnA90Map(turn_a90_map_t &map)
+{
+ DEBUG("娣诲姞鐩磋杞集鍦板浘");
+ if (prime.examing) {
+ return;
+ }
+ for (auto m: std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)) {
+ if (m.id == map.id) {
+ return;
+ }
+ }
+ std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps).push_back(map);
}
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
@@ -444,7 +420,7 @@
{
DEBUG("鍔犲叆杞﹁締淇℃伅 pointNum %d", pointNum);
- if (point == NULL || pointNum == 0 || ExamStart) return;
+ if (point == NULL || pointNum == 0 || prime.examing) return;
vector<int>().swap(CarModel.body);
vector<car_desc_t>().swap(CarModel.carDesc);
@@ -533,19 +509,19 @@
TerminateAreaExam();
- ExamStart = false;
+ prime.examing = false;
MA_SendExamStatus(0, 0);
return;
}
// type = TEST_TYPE_ROAD_CALIBRATE;
- /*if (AreaMapList.size() == 0 && type == TEST_TYPE_AREA) {
+ if (std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).size() == 0 && type == TEST_TYPE_AREA) {
DEBUG("娌℃湁鍦鸿�冨湴鍥�");
err = true;
MA_SendExamStatus(0, -1);
- }*/
- if (CarModel.carDesc.size() == 0) {
+ }
+ if (prime.pModel->carDesc.size() == 0) {
DEBUG("娌℃湁杞︽ā");
err = true;
MA_SendExamStatus(0, -2);
@@ -555,40 +531,25 @@
// err = true;
// MA_SendExamStatus(0, -3);
}
- if (type != TEST_TYPE_AREA && RoadMap.roads.size() == 0) {
- DEBUG("娌℃湁璺�冨湴鍥�");
- err = true;
- MA_SendExamStatus(0, -1);
- }
-
- if ((type == TEST_TYPE_ROAD_DUMMY_LIGHT || type == TEST_TYPE_ROAD_TRUE_LIGHT) && RoadMap.examScheme.size() == 0) {
- DEBUG("娌℃湁璺�冪嚎璺柟妗�");
- err = true;
- MA_SendExamStatus(0, -3);
- }
if (!err) {
- if (!ExamStart) {
+ if (!prime.examing) {
DEBUG("鍚姩鑰冭瘯");
ExamFaultList.clear();
examFaultIndex = 0;
- ExamStart = true;
+ prime.examing = true;
ExamType = type;
reportSeatbeltEject = false;
- if (type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
- exam_dummy_light = 0; //0
- RoadMap.calibrate = 0;
- }
- if (type == TEST_TYPE_ROAD_TRUE_LIGHT) {
- RoadMap.calibrate = 0;
- }
if (type == TEST_TYPE_AREA) {
+ DEBUG("鍊掑簱鍦板浘鏁扮洰锛�%d", std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).size());
+ DEBUG("渚ф柟鍦板浘鏁扮洰锛�%d", std::get<MAP_TYPE_PARK_EDGE>(prime.maps).size());
+ DEBUG("鏇茬嚎鍦板浘鏁扮洰锛�%d", std::get<MAP_TYPE_CURVE>(prime.maps).size());
+ DEBUG("鍧￠亾鍦板浘鏁扮洰锛�%d", std::get<MAP_TYPE_UPHILL>(prime.maps).size());
+ DEBUG("鐩磋鍦板浘鏁扮洰锛�%d", std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps).size());
+
InitAreaExam();
- }
- if (type == TEST_TYPE_ROAD_CALIBRATE) {
- RoadMap.calibrate = 1;
}
}
MA_SendExamStatus(1, 0);
@@ -607,16 +568,23 @@
realtimeMotionStatus = CalcMotionState(RtkInfoList);
// 绱姞閲岀▼
UpdataOdo(realtimeMotionStatus);
+ prime.odo = ReadOdo();
// 璁＄畻褰撳墠杞﹁締鐐瑰潗鏍囧��
- prime.prev_modeling_index = prime.curr_modeling_index;
- prime.curr_modeling_index = (prime.curr_modeling_index+1) % (sizeof (realtimeBodyModeling) / sizeof (realtimeBodyModeling[0]));
- CalcBodyModeling(realtimeBodyModeling[prime.curr_modeling_index], CarModel, RtkInfoList.front());
- // 鍚慤I涓婃姤杞﹁締鐐瑰潗鏍�
- UploadModeling(realtimeMotionStatus, realtimeBodyModeling[prime.curr_modeling_index]);
- // 瑙﹀彂鑰冭瘯椤圭洰
- if (ExamStart) {
- ExecuteExam(prime);
+ if (CarModel.carDesc.size() > 0) {
+ prime.prev_modeling_index = prime.curr_modeling_index;
+ DEBUG("缂撳瓨鍧愭爣 %d", prime.prev_modeling_index);
+ prime.curr_modeling_index = (prime.curr_modeling_index + 1) %
+ (sizeof(realtimeBodyModeling) /
+ sizeof(realtimeBodyModeling[0]));
+ DEBUG("褰撳墠鍧愭爣 %d", prime.curr_modeling_index);
+ CalcBodyModeling(realtimeBodyModeling[prime.curr_modeling_index], CarModel,
+ RtkInfoList.front());
+ DEBUG("褰撳墠鍧愭爣 鐐规暟 %d", realtimeBodyModeling[prime.curr_modeling_index].points.size());
+ // 鍚慤I涓婃姤杞﹁締鐐瑰潗鏍�
+ UploadModeling(realtimeMotionStatus, realtimeBodyModeling[prime.curr_modeling_index]);
}
+ // 瑙﹀彂鑰冭瘯椤圭洰
+ ExecuteExam(prime);
}
}
@@ -637,13 +605,14 @@
// 鏌ユ壘1绉掑墠鐨勭偣,濡傛灉鎵句笉鍒板垯瑙嗕负鍋滆溅
auto prev = s.begin();
+
std::advance(prev, 1);
for (; prev != s.end(); ++prev) {
- if (prev->utc_time - curr->utc_time > 3000) {
+ if (curr->utc_time - prev->utc_time > 3000) {
return motion;
}
- if (prev->utc_time - curr->utc_time >= 1000) {
+ if (curr->utc_time - prev->utc_time >= 1000) {
break;
}
}
@@ -713,6 +682,70 @@
return motion;
}
+void UpdateSensor(int id, int value)
+{
+ switch (id) {
+ case OBD_SPEED:
+ prime.sensor.speed = value;
+ break;
+ case ENGINE_RPM:
+ prime.sensor.rpm = value;
+ break;
+ case GEAR:
+ prime.sensor.gear = value;
+ break;
+ case TURN_SIGNAL_LAMP:
+ prime.sensor.turn_signal_lamp = value;
+ break;
+ case DIPPED_BEAM_LAMP:
+ prime.sensor.dipped_beam_lamp = value;
+ break;
+ case FOG_LAMP:
+ prime.sensor.fog_lamp = value;
+ break;
+ case CLEARANCE_LAMP:
+ prime.sensor.clearance_lamp = value;
+ break;
+ case FLASH_BEAM_LAMP:
+ prime.sensor.flash_beam_lamp = value;
+ break;
+ case MAIN_BEAM_LAMP:
+ prime.sensor.main_beam_lamp = value;
+ break;
+ case SEATBELT:
+ prime.sensor.seatbelt = value;
+ break;
+ case ENGINE_START:
+ prime.sensor.engine_start = value;
+ break;
+ case BRAKE:
+ prime.sensor.brake = value;
+ break;
+ case HAND_BRAKE:
+ prime.sensor.hand_brake = value;
+ break;
+ case SECOND_BRAKE:
+ prime.sensor.second_brake = value;
+ break;
+ case DOOR:
+ prime.sensor.door = value;
+ break;
+ case SURROUND_CAR_1:
+ prime.sensor.surround_car_1 = value;
+ break;
+ case SURROUND_CAR_2:
+ prime.sensor.surround_car_2 = value;
+ break;
+ case SURROUND_CAR_3:
+ prime.sensor.surround_car_3 = value;
+ break;
+ case SURROUND_CAR_4:
+ prime.sensor.surround_car_4 = value;
+ break;
+ default:break;
+ }
+}
+
void UpdateRTKInfo(const rtk_info_t *s)
{
RtkInfoList.push_front(*s);
@@ -769,18 +802,37 @@
static double startCarMoveDistance;
static move_status_t prevMove = STOP;
+ // 鏇存柊杞﹁締浼犳劅鍣ㄦ暟鎹�
+ DEBUG("GEAR = %d", ReadCarStatus(GEAR));
+ DEBUG("RPM = %d", ReadCarStatus(ENGINE_RPM));
+ DEBUG("TURN_SIGNAL_LAMP = %d", ReadCarStatus(TURN_SIGNAL_LAMP));
+ DEBUG("DIPPED_BEAM_LAMP = %d", ReadCarStatus(DIPPED_BEAM_LAMP));
+ DEBUG("FOG_LAMP = %d", ReadCarStatus(FOG_LAMP));
+ DEBUG("CLEARANCE_LAMP = %d", ReadCarStatus(CLEARANCE_LAMP));
+ DEBUG("FLASH_BEAM_LAMP = %d", ReadCarStatus(FLASH_BEAM_LAMP));
+ DEBUG("MAIN_BEAM_LAMP = %d", ReadCarStatus(MAIN_BEAM_LAMP));
+ DEBUG("SEATBELT = %d", ReadCarStatus(SEATBELT));
+ DEBUG("BRAKE = %d", ReadCarStatus(BRAKE));
+ DEBUG("HAND_BRAKE = %d", ReadCarStatus(HAND_BRAKE));
+ DEBUG("SECOND_BRAKE = %d", ReadCarStatus(SECOND_BRAKE));
+ DEBUG("DOOR = %d", ReadCarStatus(DOOR));
+
+ if (!prime.examing) {
+ return;
+ }
+
if (prime.pMotion->move != STOP) {
- if (ReadCarStatus(SEATBELT) == EJECT_SEATBELT && !reportSeatbeltEject) {
+ if (prime.sensor.seatbelt == EJECT && !reportSeatbeltEject) {
DEBUG("涓嶇郴瀹夊叏甯�");
reportSeatbeltEject = true;
AddExamFault(ExamType == TEST_TYPE_AREA? 10101: 30101);
}
if (rec) {
- if (!handBreakActive2 && ReadOdo() - startCarMoveDistance >= examParam.start_car_limit_distance) {
+ if (!handBreakActive2 && prime.odo - startCarMoveDistance >= prime.examParam.start_car_limit_distance) {
handBreakActive2 = true;
if (ExamType == TEST_TYPE_ROAD_DUMMY_LIGHT || ExamType == TEST_TYPE_ROAD_TRUE_LIGHT) {
- if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
+ if (prime.sensor.hand_brake == ACTIVE) {
DEBUG("Handbreak active move over 10m");
// 鎵嬪埞鎷夎捣鐘舵�佷笅锛岃椹朵簡10绫充互涓婏紝涓嶅悎鏍�
AddExamFault(40205);
@@ -790,7 +842,7 @@
AddExamFault(40206);
}
}
- } else if (!handBreakActive && ReadOdo() - startCarMoveDistance >= examParam.open_door_drive_allow_distance && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
+ } else if (!handBreakActive && prime.odo - startCarMoveDistance >= prime.examParam.open_door_drive_allow_distance && prime.sensor.hand_brake == ACTIVE) {
handBreakActive = true;
if (ExamType == TEST_TYPE_AREA) {
@@ -802,7 +854,7 @@
} else if (!rec || prevMove != STOP) { // 璁板綍鍋滆溅鐐�
rec = true;
handBreakActive = handBreakActive2 = false;
- startCarMoveDistance = ReadOdo();
+ startCarMoveDistance = prime.odo;
}
prevMove = prime.pMotion->move;
@@ -812,7 +864,7 @@
static void EngineStartHold(apptimer_var_t val) {
DEBUG("鐐圭伀瓒呮椂");
- if (ReadCarStatus(ENGINE_START) == ENGINE_START_ACTIVE) {
+ if (ReadCarStatus(ENGINE_START) == ACTIVE) {
// 涓嶅強鏃舵澗寮�鍚姩寮�鍏筹紝鎵�10鍒�
if (ExamType == TEST_TYPE_AREA) {
AddExamFault(10201);
@@ -826,16 +878,10 @@
{
struct ExamFault fault;
- if (!ExamStart)
- return;
-
fault.sn = examFaultIndex++;
- strcpy(fault.utc, StringUtil::FormatUTCTime(AppTimer_GetGmtTickCount()).c_str());
-
-// if (ExamType != TEST_TYPE_AREA) {
-// wrong += 1000;
-// }
+ //strcpy(fault.utc, StringUtil::FormatUTCTime(AppTimer_GetGmtTickCount()).c_str());
+ strcpy(fault.utc, "20230413172712.20");
fault.wrong_id = wrong;
@@ -921,10 +967,3 @@
}
}
-
-void SensorXChanged(uint16_t id, int value)
-{
-// handlePrepare(id, value);
-// handleLigthExam(id, value);
-}
-
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index f773daa..a42c989 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -8,6 +8,10 @@
#include <vector>
#include <string>
#include <list>
+#include <map>
+#include <tuple>
+#include <atomic>
+
#include "test_common/Geometry.h"
#include "rtk_module/rtk.h"
#include "map.h"
@@ -28,7 +32,7 @@
MAP_TYPE_UPHILL,
MAP_TYPE_PARK_EDGE,
MAP_TYPE_CURVE,
- MAP_TYPE_TURN_90
+ MAP_TYPE_RIGHT_CORNER
} map_type_t;
typedef struct {
@@ -80,146 +84,31 @@
typedef struct {
map_type_t type;
- int map_idx; // 瀵瑰簲绉戠洰鍦板浘闆嗗悎涓殑搴忓彿
+ int idx; // 瀵瑰簲绉戠洰鍦板浘闆嗗悎涓殑搴忓彿
+ int stage; // 瀵逛簬鍊掕溅鍏ュ簱绛夐」鐩細鏈夊涓搷浣滈樁娈�
} map_t;
typedef struct {
- bool examing = false;
- int curr_modeling_index = -1;
- int prev_modeling_index = -1;
- area_map_t *pMap;
- car_model_t *pModel;
- modeling_t *pModeling;
- motion_t *pMotion;
- map_t curr_exam_map;
- map_t arriving_map;
-} prime_t;
-
-struct ExamFault {
- int sn;
- char utc[32];
- int wrong_id;
-};
-
-#define LINE_DOTTED 0
-#define LINE_SOLID 1
-#define LINE_HALF_SOLID_LEFT 2
-#define LINE_HALF_SOLID_RIGHT 3
-#define LINE_BOUNDARY 4
-
-//杞﹂亾鏂瑰悜锛堟寜浣嶇粍鍚堬級,濡傛灉涓�0锛屽垯琛ㄦ棤杞﹂亾鏂瑰悜璇存槑锛�
-#define LANE_FORWARD 0x01
-#define LANE_LEFT 0x02
-#define LANE_RIGHT 0x04
-#define LANE_BACKWARD 0x08
-
-#define ZEBRA_CROSSING 0
-#define SCHOOL_AREA 1
-#define BUS_STATION_AREA 2
-#define GRID_AREA 3
-
-#define ROAD_ACTIVE_FORWARD LANE_FORWARD
-#define ROAD_ACTIVE_TURN_LEFT LANE_LEFT
-#define ROAD_ACTIVE_TURN_RIGHT LANE_RIGHT
-#define ROAD_ACTIVE_TURN_BACKWARD LANE_BACKWARD
-
-typedef struct {
- int character; // 灞炴�э紙瀹炵嚎銆佽櫄绾匡紝鏈変簺鍙互鎺夊ご鐨勮矾娈碉級
- std::vector<PointF> points; //
-} edge_t;
-
-typedef struct {
- int character; // 灞炴�с�婂疄绾裤�佽櫄绾裤�佸崐瀹炲崐铏氱嚎銆�
- std::vector<PointF> points;
-} segment_t;
-
-typedef struct {
- PointF start;
- PointF end;
- std::vector<int> direct;
-} lane_direct_t;
-
-// 涓�缁勫钩琛岀殑鍒嗛亾绾�
-typedef struct {
- std::vector<lane_direct_t> lane_direct; // 涓�缁勫鍚戠嚎鐨勯厤缃�
- std::vector<std::vector<segment_t>> lines; // 姣忔鍩熶笅鐨勫钩琛岀殑涓�缁勭嚎
-} separate_t;
-
-typedef struct {
- bool stopFlag;
- Line line;
- bool centrePointValid;
- PointF centrePoint;
-} stop_line_t;
-
-typedef struct {
- int id;
-// Line stopLine;
-// int active; // 鍒拌揪璺彛灏鹃儴鐨勮杩涙柟鍚�
- bool activeBreak; // 璺彛鍒硅溅鍑忛��
- bool activeStop; // 璺彛鍋滆溅鐬湜
- bool errorLane; // 閿欒杞﹂亾
- int targetRoad;
-// int stopFlag;
-// string tts;
-// bool arrivedTail;
- Polygon area; // 鏁翠釜閬撹矾鍖哄煙
-
- std::vector<stop_line_t> stopLine;
- std::vector<edge_t> leftEdge;
- std::vector<edge_t> rightEdge;
- std::vector<separate_t> separate;
-} road_t;
-
-typedef struct {
- int id;
- int road;
- int type;
- bool activeBreak;
- bool overSpeed;
- std::vector<PointF> area; // 浜鸿閬撶瓑鍙充晶2鐐癸紝缃戞牸绾�4鐐�
- std::vector<PointF> leftPoints; // 瀵瑰簲鍒伴亾璺乏渚х殑鐐�
-} special_area_t;
-
-typedef struct {
- int road;
- int active;
- std::vector<PointF> points;
-} trigger_line_t;
-
-typedef struct {
- int id;
- int type;
- std::vector<PointF> points;
-} forbid_line_t;
-
-typedef struct {
- int road_id;
- int index;
- int active;
-} crossing_active_t;
-
-typedef struct {
- std::string name;
- std::vector<crossing_active_t> crossingActive;
- std::vector<trigger_line_t> triggerLines;
-} scheme_t;
-
-struct road_exam_map {
- int calibrate;
- std::vector<road_t> roads;
- std::vector<special_area_t> specialAreas;
-// std::vector<trigger_line_t> triggerLines;
- std::vector<forbid_line_t> forbidLines;
- std::vector<scheme_t> examScheme;
-};
-
-struct area_exam_map {
- int id;
- int type;
- Polygon map;
- Polygon map2;
-};
+ std::atomic_int speed;
+ std::atomic_int rpm;
+ std::atomic_int gear;
+ std::atomic_int turn_signal_lamp;
+ std::atomic_int dipped_beam_lamp;
+ std::atomic_int fog_lamp;
+ std::atomic_int clearance_lamp;
+ std::atomic_int flash_beam_lamp;
+ std::atomic_int main_beam_lamp;
+ std::atomic_int seatbelt;
+ std::atomic_int engine_start;
+ std::atomic_int brake;
+ std::atomic_int hand_brake;
+ std::atomic_int second_brake;
+ std::atomic_int door;
+ std::atomic_int surround_car_1;
+ std::atomic_int surround_car_2;
+ std::atomic_int surround_car_3;
+ std::atomic_int surround_car_4;
+} sensor_t;
typedef struct {
int hold_start_key_limit_time; // 鐐圭伀淇濇寔锛屾绉�
@@ -304,20 +193,38 @@
std::string crossing_turn_unknown_tts;
} exam_param_t;
-extern exam_param_t examParam;
+typedef struct {
+ bool examing = false;
+ int curr_modeling_index = -1;
+ int prev_modeling_index = -1;
+ int arriving_map = -1;
+ std::tuple<int, std::vector<park_button_map_t>, std::vector<uphill_map_t>, std::vector<park_edge_map_t>, std::vector<curve_map_t>, std::vector<turn_a90_map_t>> maps;
+ car_model_t *pModel;
+ modeling_t *pModeling;
+ motion_t *pMotion;
+ map_t examing_area;
+ sensor_t sensor;
+ exam_param_t examParam;
+ double odo; // 鐢≧TK瀹氫綅璁＄畻鐨勮椹堕噷绋�
+} prime_t;
+struct ExamFault {
+ int sn;
+ char utc[32];
+ int wrong_id;
+};
+
+prime_t& GetPrime(void);
void DriverTestInit(void);
void ReadDriverExamPrimer(void);
void ClearAreaMap(void);
-void AddAreaMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2);
+
void AddCurveMap(curve_map_t &map);
void AddParkButtonMap(park_button_map_t &map);
void AddParkEdgeMap(park_edge_map_t &map);
-
-void CleanRoadMap(void);
-void SetRoadMap(road_exam_map &map, std::vector<scheme_t> &scheme);
-void SetRoadExamScheme(std::vector<scheme_t> &scheme);
+void AddUphillMap(uphill_map_t &map);
+void AddTurnA90Map(turn_a90_map_t &map);
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
@@ -325,19 +232,13 @@
void StartDriverExam(int start, int type);
+void UpdateSensor(int id, int value);
void UpdateRTKInfo(const rtk_info_t *s);
void AddExamFault(int wrong);
-void RoadChange(int road, int status);
-void CrossingChange(int road, int crossing, int status);
+
void SystemShutdown(int event, int timeout);
-void SetDummyLightExam(int n, struct dummy_light_exam *cfg);
-
-void MasterInqRoadStatus(void);
-
int CorrectPauseCriteria(int src);
-
-void SensorXChanged(uint16_t id, int value);
#endif //RTKDRIVERTEST_DRIVER_TEST_H
diff --git a/lib/src/main/cpp/map.h b/lib/src/main/cpp/map.h
index 6b40fec..8032c69 100644
--- a/lib/src/main/cpp/map.h
+++ b/lib/src/main/cpp/map.h
@@ -13,28 +13,28 @@
int id;
std::string name;
double line_width;
- std::vector<PointF> map;
+ std::vector<PointF> points;
} park_button_map_t;
typedef struct {
int id;
std::string name;
double line_width;
- std::vector<PointF> map;
+ std::vector<PointF> points;
} park_edge_map_t;
typedef struct {
int id;
std::string name;
double line_width;
- std::vector<PointF> map;
+ std::vector<PointF> points;
} turn_a90_map_t;
typedef struct {
int id;
std::string name;
double line_width;
- std::vector<PointF> map;
+ std::vector<PointF> points;
} uphill_map_t;
typedef struct {
@@ -54,13 +54,5 @@
PointF back_half_small_circle_centre;
double back_half_small_circle_radius;
} curve_map_t;
-
-typedef struct {
- std::vector<curve_map_t> curve_map;
- std::vector<park_button_map_t> park_button_map;
- std::vector<park_edge_map_t> park_edge_map;
- std::vector<turn_a90_map_t> turn_a90_map;
- std::vector<uphill_map_t> uphill_map;
-} area_map_t;
#endif //MYAPPLICATION3_MAP_H
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index 486fa55..1415432 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -87,7 +87,7 @@
#define MA_OUT_DBG_INFO 0x0010
-static int OnOff = 0;//0xFFFF;
+static int OnOff = 0xFFFF;
struct msg_2_main_t {
int cmd;
@@ -115,6 +115,8 @@
}
lock_guard<mutex> lock(msg_mutex);
+
+// DEBUG("鍔犲叆浼犺緭闃熷垪 0x%04X, 褰撳墠闀垮害 %d", cmd, MessageBuffer.size());
MessageBuffer.push_front(msg);
@@ -164,6 +166,7 @@
{
if (v.IsArray()) {
PointF point;
+
for (int i = 0; i < v.Size();) {
if (v.Size() - i >= 2) {
point.X = v[i].GetDouble();
@@ -359,9 +362,9 @@
writer.Int(brief->horn);
writer.Key("wiper");
writer.Int(brief->wiper);
- writer.Key("hand_break");
+ writer.Key("hand_brake");
writer.Int(brief->handBreak);
- writer.Key("main_break");
+ writer.Key("brake");
writer.Int(brief->mainBreak);
writer.Key("left_turn_lamp");
writer.Int(brief->leftTurnLamp);
@@ -740,686 +743,10 @@
ConfigPlatform(&cfg);
break;
}
- case ID_MS_ROAD_MAP: {
- Document doc;
- doc.Parse(value);
-
- if (!doc.HasParseError()) {
- CleanRoadMap();
-
- DEBUG("寮�濮嬭В鏋愯矾鑰冨湴鍥�");
-
- vector<double> mapPoints;
- mapPoints.clear();
-
- if (doc.HasMember("points")) {
- const Value &s = doc["points"];
-
- DEBUG("寰楀埌鎵�鏈夌偣");
- // X-Y鍧愭爣闆嗗悎
- for (Value::ConstValueIterator itr2 = s.Begin();
- itr2 != s.End(); ++itr2) {
- mapPoints.push_back((*itr2).GetDouble());
- }
-
- }
-
- if (doc.HasMember("maps")) {
- const Value &a = doc["maps"];
-
- for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) {
-
- DEBUG("寰楀埌鍚勫瓙鍦板浘");
- if (itr->IsObject()) {
- // a Map
- int id, type;
- int stop_flag = 0;
- string tts;
-
- vector<vector<int>> redLines;
- vector<vector<int>> greenLines;
- vector<vector<int>> triggerLines;
- vector<vector<int>> redAreas;
- vector<vector<int>> roadEdgeLines;
-
- vector<int> area;
- vector<int> stopLine;
-
- roadEdgeLines.clear();
- stopLine.clear();
- area.clear();
- tts.clear();
- redLines.clear();
- greenLines.clear();
- triggerLines.clear();
- redAreas.clear();
-
- vector<int> points;
-
- if (itr->HasMember("red_line")) {
- const Value &s = (*itr)["red_line"];
-
- for (Value::ConstValueIterator itrLine = s.Begin();
- itrLine != s.End(); ++itrLine) {
- points.clear();
- for (Value::ConstValueIterator itrPoint = (*itrLine).Begin();
- itrPoint != (*itrLine).End(); ++itrPoint) {
- points.push_back((*itrPoint).GetInt());
- }
- redLines.push_back(points);
- }
- }
-
- if (itr->HasMember("green_line")) {
- const Value &s = (*itr)["green_line"];
-
- for (Value::ConstValueIterator itrLine = s.Begin();
- itrLine != s.End(); ++itrLine) {
- points.clear();
- for (Value::ConstValueIterator itrPoint = (*itrLine).Begin();
- itrPoint != (*itrLine).End(); ++itrPoint) {
- points.push_back((*itrPoint).GetInt());
- }
- greenLines.push_back(points);
- }
- }
-
- if (itr->HasMember("all_trigger_line")) {
- const Value &s = (*itr)["all_trigger_line"];
-
- for (Value::ConstValueIterator itrLine = s.Begin();
- itrLine != s.End(); ++itrLine) {
- points.clear();
- for (Value::ConstValueIterator itrPoint = (*itrLine).Begin();
- itrPoint != (*itrLine).End(); ++itrPoint) {
- points.push_back((*itrPoint).GetInt());
- }
- triggerLines.push_back(points);
- }
- }
-
- if (itr->HasMember("red_area")) {
- const Value &s = (*itr)["red_area"];
-
- for (Value::ConstValueIterator itrLine = s.Begin();
- itrLine != s.End(); ++itrLine) {
- points.clear();
- for (Value::ConstValueIterator itrPoint = (*itrLine).Begin();
- itrPoint != (*itrLine).End(); ++itrPoint) {
- points.push_back((*itrPoint).GetInt());
- }
- redAreas.push_back(points);
- }
- }
-
- if (itr->HasMember("road_edge_line")) {
- const Value &s = (*itr)["road_edge_line"];
-
- for (Value::ConstValueIterator itrLine = s.Begin();
- itrLine != s.End(); ++itrLine) {
- points.clear();
- for (Value::ConstValueIterator itrPoint = (*itrLine).Begin();
- itrPoint != (*itrLine).End(); ++itrPoint) {
- points.push_back((*itrPoint).GetInt());
- }
- roadEdgeLines.push_back(points);
- }
- }
-
- if (itr->HasMember("area")) {
- const Value &s = (*itr)["area"];
-
- for (Value::ConstValueIterator itrPoint = s.Begin();
- itrPoint != s.End(); ++itrPoint) {
- area.push_back((*itrPoint).GetInt());
- }
- }
-
- if (itr->HasMember("stop_line")) {
- const Value &s = (*itr)["stop_line"];
-
- for (Value::ConstValueIterator itrPoint = s.Begin();
- itrPoint != s.End(); ++itrPoint) {
- stopLine.push_back((*itrPoint).GetInt());
- }
- }
-
- if (itr->HasMember("id")) {
- const Value &s = (*itr)["id"];
- id = s.GetInt();
- }
-
- if (itr->HasMember("item")) {
- const Value &s = (*itr)["item"];
- type = s.GetInt();
- }
-
- if (itr->HasMember("stop_flag")) {
- const Value &s = (*itr)["stop_flag"];
- stop_flag = s.GetInt();
- }
-
- if (itr->HasMember("tts")) {
- const Value &s = (*itr)["tts"];
- tts = s.GetString();
- }
-
- }
- }
- }
- } else {
- DEBUG("############## 鍦板浘瑙ｆ瀽鍑洪敊###################");
- }
- break;
- }
- case ID_MS_ROAD_MAP2: {
- Document doc;
- doc.Parse(value);
- if (!doc.HasParseError()) {
- DEBUG("寮�濮嬭В鏋愯矾鑰冨湴鍥�");
-
- vector<PointF> mapPoints;
- mapPoints.clear();
-
- if (doc.HasMember("points")) {
- const Value &s = doc["points"];
-
- DEBUG("寰楀埌鎵�鏈夌偣 鍏� %d", s.Size()/2);
-
- int n = 0;
- PointF temp;
-
- // X-Y鍧愭爣闆嗗悎
- for (Value::ConstValueIterator itr = s.Begin();
- itr != s.End(); ++itr, ++n) {
- if ((n % 2) == 0) {
- temp.X = (*itr).GetDouble();
- } else {
- temp.Y = (*itr).GetDouble();
- mapPoints.push_back(temp);
- }
- }
- }
-
- road_exam_map map;
-
- if (doc.HasMember("road")) {
- const Value &a = doc["road"];
-
- if (a.IsArray()) {
- for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) {
- road_t road;
-
- if (itr->HasMember("id")) {
- const Value &s = (*itr)["id"];
- DEBUG("璺痠d %d", s.GetInt());
- road.id = s.GetInt();
- }
- if (itr->HasMember("crossing")) {
- const Value &a2 = (*itr)["crossing"];
-
- vector<stop_line_t> crossing;
-
-
- for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2) {
- stop_line_t temp;
- if (!itr2->IsObject()) {
- break;
- }
- if (itr2->HasMember("stop_flag")) {
- const Value &s = (*itr2)["stop_flag"];
- temp.stopFlag = s.GetInt();
- DEBUG("璺彛鍋滆溅 %d", temp.stopFlag);
- } else {
- temp.stopFlag = false;
- }
- if (itr2->HasMember("line")) {
- const Value &s = (*itr2)["line"];
- PointF p1, p2;
- int n = 0;
-
- if (s.IsArray() && s.Size() >= 2) {
- for (Value::ConstValueIterator itr3 = s.Begin(); itr3 != s.End(); ++itr3, ++n) {
- if (n == 0) {
- p1 = mapPoints[(*itr3).GetInt()];
- } else if (n == 1) {
- p2 = mapPoints[(*itr3).GetInt()];
- }
- }
- MakeLine(&temp.line, &p1, &p2);
- }
- }
- if (itr2->HasMember("center_point")) {
- const Value &s = (*itr2)["center_point"];
- temp.centrePoint = mapPoints[s.GetInt()];
- temp.centrePointValid = true;
- } else {
- temp.centrePointValid = false;
- }
- crossing.push_back(temp);
- }
-
- road.stopLine.assign(crossing.begin(), crossing.end());
- }
-
- if (itr->HasMember("next_road")) {
- const Value &s = (*itr)["next_road"];
- road.targetRoad = s.GetInt();
- }
- if (itr->HasMember("left_edge")) {
- const Value &a2 = (*itr)["left_edge"];
- DEBUG("宸﹁竟绾�");
-
- if (a2.IsArray()) {
- for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2) {
- edge_t edge;
-
- if (itr2->HasMember("type")) {
- const Value &s = (*itr2)["type"];
- DEBUG("\t绫诲瀷 %d", s.GetInt());
-
- edge.character = s.GetInt();
- }
- if (itr2->HasMember("line")) {
- const Value &a3 = (*itr2)["line"];
- if (a3.IsArray()) {
- for (Value::ConstValueIterator itr3 = a3.Begin(); itr3 != a3.End(); ++itr3) {
- DEBUG("\t绾跨偣 %d", (*itr3).GetInt());
- edge.points.push_back(mapPoints[(*itr3).GetInt()]);
- }
- }
- }
-
- road.leftEdge.push_back(edge);
- }
- }
- }
- if (itr->HasMember("right_edge")) {
- const Value &a2 = (*itr)["right_edge"];
- DEBUG("鍙宠竟绾�");
- if (a2.IsArray()) {
- for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2) {
- edge_t edge;
-
- if (itr2->HasMember("type")) {
- const Value &s = (*itr2)["type"];
- DEBUG("\t绫诲瀷 %d", s.GetInt());
- edge.character = s.GetInt();
- }
- if (itr2->HasMember("line")) {
- const Value &a3 = (*itr2)["line"];
- if (a3.IsArray()) {
- for (Value::ConstValueIterator itr3 = a3.Begin(); itr3 != a3.End(); ++itr3) {
- DEBUG("\t绾跨偣 %d", (*itr3).GetInt());
- edge.points.push_back(mapPoints[(*itr3).GetInt()]);
- }
- }
- }
-
- road.rightEdge.push_back(edge);
- }
- }
- }
- if (itr->HasMember("separate")) {
- const Value &a2 = (*itr)["separate"];
- DEBUG("鍒嗛亾鏁伴噺 %d", a2.Size());
-
- for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2) {
- separate_t sep;
- if (!itr2->IsObject())
- break;
-
- if (itr2->HasMember("lane_guide")) {
- const Value &a3 = (*itr2)["lane_guide"];
-
- for (Value::ConstValueIterator itr3 = a3.Begin(); itr3 != a3.End(); ++itr3) {
- lane_direct_t temp;
-
- if (itr3->HasMember("head_tail")) {
- const Value &a4 = (*itr3)["head_tail"];
- int n = 0;
-
- for (Value::ConstValueIterator itr4 = a4.Begin(); itr4 != a4.End(); ++itr4, ++n) {
- if (n == 0)
- temp.start = mapPoints[(*itr4).GetInt()];
- else if (n == 1)
- temp.end = mapPoints[(*itr4).GetInt()];
- }
- }
- if (itr3->HasMember("guide")) {
- const Value &a4 = (*itr3)["guide"];
-
- for (Value::ConstValueIterator itr4 = a4.Begin(); itr4 != a4.End(); ++itr4) {
- temp.direct.push_back((*itr4).GetInt());
- }
- }
- DEBUG("\t寰楀埌涓�缁勫鍚戠嚎");
- sep.lane_direct.push_back(temp);
- }
- }
- if (itr2->HasMember("lane_line")) {
- const Value &a3 = (*itr2)["lane_line"];
- DEBUG("\t绾挎暟閲� %d", a3.Size());
- for (Value::ConstValueIterator itr3 = a3.Begin(); itr3 != a3.End(); ++itr3) {
- DEBUG("\t\t鑺傛暟閲� %d", (*itr3).Size());
- vector<segment_t> sline;
-
- for (Value::ConstValueIterator itr4 = (*itr3).Begin(); itr4 != (*itr3).End(); ++itr4) {
- const Value &type = (*itr4)["type"];
- const Value &line = (*itr4)["line"];
-
- segment_t seg;
-
- DEBUG("\t\t\t鑺傜被鍨� = %d", type.GetInt());
- seg.character = type.GetInt();
-
- for (Value::ConstValueIterator itr5 = line.Begin(); itr5 != line.End(); ++itr5) {
- DEBUG("\t\t\t鐐� = %d", (*itr5).GetInt());
-
- seg.points.push_back(mapPoints[(*itr5).GetInt()]);
- }
-
- sline.push_back(seg);
- }
- sep.lines.push_back(sline);
- }
- }
- road.separate.push_back(sep);
- }
- }
-
- map.roads.push_back(road);
- }
- }
- }
-
- if (doc.HasMember("special_area")) {
- const Value &a = doc["special_area"];
-
- for (Value::ConstValueIterator itr = a.Begin();
- itr != a.End(); ++itr) {
- special_area_t specialArea;
-
- if (itr->HasMember("type")) {
- const Value &s = (*itr)["type"];
- specialArea.type = s.GetInt();
- }
- if (itr->HasMember("id")) {
- const Value &s = (*itr)["id"];
- specialArea.id = s.GetInt();
- }
- if (itr->HasMember("road")) {
- const Value &s = (*itr)["road"];
- specialArea.road = s.GetInt();
- }
-
- if (itr->HasMember("area")) {
- const Value &a2 = (*itr)["area"];
-
- for (Value::ConstValueIterator itr2 = a2.Begin();
- itr2 != a2.End(); ++itr2) {
- specialArea.area.push_back(mapPoints[(*itr2).GetInt()]);
- }
- }
-
- map.specialAreas.push_back(specialArea);
- }
- }
- /*if (doc.HasMember("trigger_line")) {
- const Value &a = doc["trigger_line"];
-
- for (Value::ConstValueIterator itr = a.Begin();
- itr != a.End(); ++itr) {
- trigger_line_t trigger;
-
- if (itr->HasMember("type")) {
- const Value &s = (*itr)["type"];
- trigger.active = s.GetInt();
- }
- if (itr->HasMember("id")) {
- const Value &s = (*itr)["id"];
- trigger.id = s.GetInt();
- }
- if (itr->HasMember("road")) {
- const Value &s = (*itr)["road"];
- trigger.road = s.GetInt();
- }
- if (itr->HasMember("tts")) {
- const Value &s = (*itr)["tts"];
- trigger.tts = s.GetString();
- }
- if (itr->HasMember("time")) {
- const Value &s = (*itr)["time"];
- trigger.time = s.GetInt();
- } else {
- trigger.time = 0;
- }
- if (itr->HasMember("distance")) {
- const Value &s = (*itr)["distance"];
- trigger.distance = s.GetInt();
- } else {
- trigger.distance = 0;
- }
-
- if (itr->HasMember("line")) {
- const Value &a2 = (*itr)["line"];
-
- for (Value::ConstValueIterator itr2 = a2.Begin();
- itr2 != a2.End(); ++itr2) {
- trigger.points.push_back(mapPoints[(*itr2).GetInt()]);
- }
- }
-
- map.triggerLines.push_back(trigger);
- }
- }*/
- if (doc.HasMember("red_line")) {
- const Value &a = doc["red_line"];
-
- for (Value::ConstValueIterator itr = a.Begin();
- itr != a.End(); ++itr) {
- forbid_line_t forbid;
-
- if (itr->HasMember("type")) {
- const Value &s = (*itr)["type"];
- forbid.type = s.GetInt();
-
- DEBUG("绂佹绾� type %d", forbid.type);
- }
- if (itr->HasMember("id")) {
- const Value &s = (*itr)["id"];
- forbid.id = s.GetInt();
- }
- if (itr->HasMember("points")) {
- const Value &a2 = (*itr)["points"];
-
- for (Value::ConstValueIterator itr2 = a2.Begin();
- itr2 != a2.End(); ++itr2) {
- forbid.points.push_back(mapPoints[(*itr2).GetInt()]);
- }
- }
- map.forbidLines.push_back(forbid);
- }
- }
-
- vector<scheme_t> schemes;
-
- if (doc.HasMember("scheme")) {
- const Value &a = doc["scheme"];
-
- for (Value::ConstValueIterator itr = a.Begin();
- itr != a.End(); ++itr) {
- scheme_t scheme;
- if (itr->HasMember("name")) {
- const Value &s = (*itr)["name"];
- scheme.name = s.GetString();
- }
- if (itr->HasMember("crossing_active")) {
- const Value &a2 = (*itr)["crossing_active"];
-
- for (Value::ConstValueIterator itr2 = a2.Begin();
- itr2 != a2.End(); ++itr2) {
- crossing_active_t act;
-
- if (itr2->HasMember("road")) {
- const Value &s = (*itr2)["road"];
- act.road_id = s.GetInt();
- }
- if (itr2->HasMember("idx")) {
- const Value &s = (*itr2)["idx"];
- act.index = s.GetInt();
- }
- if (itr2->HasMember("active")) {
- const Value &s = (*itr2)["active"];
- act.active = s.GetInt();
- }
-
- scheme.crossingActive.push_back(act);
- }
- }
- if (itr->HasMember("trigger_line")) {
- const Value &a2 = (*itr)["trigger_line"];
-
- for (Value::ConstValueIterator itr2 = a2.Begin();
- itr2 != a2.End(); ++itr2) {
- trigger_line_t ins;
-
- if (itr2->HasMember("x_y")) {
- const Value &a3 = (*itr2)["x_y"];
- if (a3.IsArray()) {
- PointF point;
- int n = 0;
- for (Value::ConstValueIterator itr3 = a3.Begin();
- itr3 != a3.End(); ++itr3) {
- if (n == 0) {
- point.X = (*itr3).GetDouble();
- n = 1;
- } else if (n == 1) {
- point.Y = (*itr3).GetDouble();
- ins.points.push_back(point);
- n = 0;
- }
- }
- }
- }
- if (itr2->HasMember("road")) {
- const Value &s = (*itr2)["road"];
- ins.road = s.GetInt();
- }
- if (itr2->HasMember("type")) {
- const Value &s = (*itr2)["type"];
- ins.active = s.GetInt();
- }
-
- scheme.triggerLines.push_back(ins);
- }
- }
- schemes.push_back(scheme);
- }
- }
-
- DEBUG("鍦板浘瑙ｆ瀽瀹屾瘯");
-
- CleanRoadMap();
- SetRoadMap(map, schemes);
- } else {
- DEBUG("############## 鍦板浘瑙ｆ瀽鍑洪敊###################");
- }
-
- break;
- }
- case ID_MS_SCHEME: {
- Document doc;
- doc.Parse(value);
-
- DEBUG("鏂规 %s", value);
-
- if (!doc.HasParseError()) {
- DEBUG("寮�濮嬭В鏋愯矾鑰冩柟妗�");
- vector<scheme_t> schemes;
-
- if (doc.HasMember("scheme")) {
- const Value &a = doc["scheme"];
-
- for (Value::ConstValueIterator itr = a.Begin();
- itr != a.End(); ++itr) {
- scheme_t scheme;
- if (itr->HasMember("name")) {
- const Value &s = (*itr)["name"];
- scheme.name = s.GetString();
- }
- if (itr->HasMember("crossing_active")) {
- const Value &a2 = (*itr)["crossing_active"];
-
- for (Value::ConstValueIterator itr2 = a2.Begin();
- itr2 != a2.End(); ++itr2) {
- crossing_active_t act;
-
- if (itr2->HasMember("road")) {
- const Value &s = (*itr2)["road"];
- act.road_id = s.GetInt();
- }
- if (itr2->HasMember("idx")) {
- const Value &s = (*itr2)["idx"];
- act.index = s.GetInt();
- }
- if (itr2->HasMember("active")) {
- const Value &s = (*itr2)["active"];
- act.active = s.GetInt();
- }
-
- scheme.crossingActive.push_back(act);
- }
- }
- if (itr->HasMember("trigger_line")) {
- const Value &a2 = (*itr)["trigger_line"];
-
- for (Value::ConstValueIterator itr2 = a2.Begin();
- itr2 != a2.End(); ++itr2) {
- trigger_line_t ins;
-
- if (itr2->HasMember("x_y")) {
- const Value &a3 = (*itr2)["x_y"];
- if (a3.IsArray()) {
- PointF point;
- int n = 0;
- for (Value::ConstValueIterator itr3 = a3.Begin();
- itr3 != a3.End(); ++itr3) {
- if (n == 0) {
- point.X = (*itr3).GetDouble();
- n = 1;
- } else if (n == 1) {
- point.Y = (*itr3).GetDouble();
- ins.points.push_back(point);
- n = 0;
- }
- }
- }
- }
- if (itr2->HasMember("road")) {
- const Value &s = (*itr2)["road"];
- ins.road = s.GetInt();
- }
- if (itr2->HasMember("type")) {
- const Value &s = (*itr2)["type"];
- ins.active = s.GetInt();
- }
-
- scheme.triggerLines.push_back(ins);
- }
- }
- schemes.push_back(scheme);
- }
- }
- SetRoadExamScheme(schemes);
- }
- break;
- }
-
case ID_MS_EXAM_PARAM: {
Document doc;
doc.Parse(value);
- if (!doc.HasParseError()) {
+ /*if (!doc.HasParseError()) {
if (doc.HasMember("hold_start_key_limit_time")) {
const Value &a = doc["hold_start_key_limit_time"];
examParam.hold_start_key_limit_time = a.GetInt();
@@ -1682,7 +1009,7 @@
examParam.crossing_break_valid_distance = a.GetInt();
DEBUG("crossing_break_valid_distance = %d", examParam.crossing_break_valid_distance);
}
- }
+ }*/
break;
}
@@ -1698,29 +1025,23 @@
if (a.IsArray()) {
for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) {
- // a Map
- string name;
- int id = -1, type = -1;
- //int id, type, pointNum = 0, point2Num = 0;
- //double (*map)[2] = NULL, (*map2)[2] = NULL;
-
if (itr->IsObject()) {
- if (itr->HasMember("id")) {
- const Value &s = (*itr)["id"];
- id = s.GetInt();
- }
- if (itr->HasMember("name")) {
- const Value &s = (*itr)["id"];
- name = s.GetString();
- }
+
if (itr->HasMember("item")) {
- const Value &s = (*itr)["item"];
- type = s.GetInt();
+ const Value &s = (*itr)["item"]; // 椤圭洰绫诲瀷锛氬�掑簱锛屼晶鏂瑰仠杞�...
+ int type = s.GetInt();
switch (type) {
case MAP_TYPE_CURVE: {
curve_map_t map;
- map.id = id;
+ if (itr->HasMember("id")) {
+ const Value &s2 = (*itr)["id"];
+ map.id = s2.GetInt();
+ }
+ if (itr->HasMember("name")) {
+ const Value &s2 = (*itr)["name"];
+ map.name = s2.GetString();
+ }
if (itr->HasMember("left_start_point")) {
const Value &s2 = (*itr)["left_start_point"];
LoadPoint(map.left_start_point, s2);
@@ -1737,46 +1058,117 @@
const Value &s2 = (*itr)["right_end_point"];
LoadPoint(map.right_end_point, s2);
}
- AddCurveMap(map);
+
+ if (itr->HasMember("front_half_big_circle_centre")) {
+ const Value &s2 = (*itr)["front_half_big_circle_centre"];
+ LoadPoint(map.right_end_point, s2);
+ }
+ if (itr->HasMember("front_half_big_circle_radius")) {
+ const Value &s2 = (*itr)["front_half_big_circle_radius"];
+ map.front_half_big_circle_radius = s2.GetDouble();
+ }
+ if (itr->HasMember("front_half_small_circle_centre")) {
+ const Value &s2 = (*itr)["front_half_small_circle_centre"];
+ LoadPoint(map.right_end_point, s2);
+ }
+ if (itr->HasMember("front_half_small_circle_radius")) {
+ const Value &s2 = (*itr)["front_half_small_circle_radius"];
+ map.front_half_small_circle_radius = s2.GetDouble();
+ }
+ if (itr->HasMember("back_half_big_circle_centre")) {
+ const Value &s2 = (*itr)["back_half_big_circle_centre"];
+ LoadPoint(map.right_end_point, s2);
+ }
+ if (itr->HasMember("back_half_big_circle_radius")) {
+ const Value &s2 = (*itr)["back_half_big_circle_radius"];
+ map.back_half_big_circle_radius = s2.GetDouble();
+ }
+ if (itr->HasMember("back_half_small_circle_centre")) {
+ const Value &s2 = (*itr)["back_half_small_circle_centre"];
+ LoadPoint(map.right_end_point, s2);
+ }
+ if (itr->HasMember("back_half_small_circle_radius")) {
+ const Value &s2 = (*itr)["back_half_small_circle_radius"];
+ map.back_half_small_circle_radius = s2.GetDouble();
+ }
+ //AddCurveMap(map);
break;
}
case MAP_TYPE_PARK_BUTTOM: {
park_button_map_t map;
-
- map.id = id;
+ if (itr->HasMember("id")) {
+ const Value &s2 = (*itr)["id"];
+ map.id = s2.GetInt();
+ }
+ if (itr->HasMember("name")) {
+ const Value &s2 = (*itr)["name"];
+ map.name = s2.GetString();
+ }
+ if (itr->HasMember("line_width")) {
+ const Value &s2 = (*itr)["line_width"];
+ map.line_width = s2.GetDouble();
+ }
if (itr->HasMember("point") && (*itr)["point"].IsArray()) {
- const Value &arr = (*itr)["point"];
- LoadPoints(map.map, arr);
+ const Value &arr = (*itr)["point"][0]["x-y"];
+
+ LoadPoints(map.points, arr);
}
AddParkButtonMap(map);
break;
}
case MAP_TYPE_PARK_EDGE: {
park_edge_map_t map;
- map.id = id;
+ if (itr->HasMember("id")) {
+ const Value &s2 = (*itr)["id"];
+ map.id = s2.GetInt();
+ }
+ if (itr->HasMember("name")) {
+ const Value &s2 = (*itr)["name"];
+ map.name = s2.GetString();
+ }
+ if (itr->HasMember("line_width")) {
+ const Value &s2 = (*itr)["line_width"];
+ map.line_width = s2.GetDouble();
+ }
if (itr->HasMember("point") && (*itr)["point"].IsArray()) {
- const Value &arr = (*itr)["point"];
- LoadPoints(map.map, arr);
+ const Value &arr = (*itr)["point"][0]["x-y"];
+ LoadPoints(map.points, arr);
}
AddParkEdgeMap(map);
break;
}
case MAP_TYPE_UPHILL: {
uphill_map_t map;
- map.id = id;
- if (itr->HasMember("point") && (*itr)["point"].IsArray()) {
- const Value &arr = (*itr)["point"];
- LoadPoints(map.map, arr);
+ if (itr->HasMember("id")) {
+ const Value &s2 = (*itr)["id"];
+ map.id = s2.GetInt();
}
+ if (itr->HasMember("name")) {
+ const Value &s2 = (*itr)["name"];
+ map.name = s2.GetString();
+ }
+ if (itr->HasMember("point") && (*itr)["point"].IsArray()) {
+ const Value &arr = (*itr)["point"][0]["x-y"];
+ LoadPoints(map.points, arr);
+ }
+ AddUphillMap(map);
break;
}
- case MAP_TYPE_TURN_90: {
+ case MAP_TYPE_RIGHT_CORNER: {
turn_a90_map_t map;
- map.id = id;
- if (itr->HasMember("point") && (*itr)["point"].IsArray()) {
- const Value &arr = (*itr)["point"];
- LoadPoints(map.map, arr);
+ if (itr->HasMember("id")) {
+ const Value &s2 = (*itr)["id"];
+ map.id = s2.GetInt();
}
+ if (itr->HasMember("name")) {
+ const Value &s2 = (*itr)["name"];
+ map.name = s2.GetString();
+ }
+ if (itr->HasMember("point") && (*itr)["point"].IsArray()) {
+ const Value &arr = (*itr)["point"][0]["x-y"];
+ LoadPoints(map.points, arr);
+ }
+ AddTurnA90Map(map);
break;
}
default:
diff --git a/lib/src/main/cpp/master/comm_if.h b/lib/src/main/cpp/master/comm_if.h
index 8a13a4d..d2b9405 100644
--- a/lib/src/main/cpp/master/comm_if.h
+++ b/lib/src/main/cpp/master/comm_if.h
@@ -141,7 +141,7 @@
void MA_SendGpsBrief(const struct gpsBrief *brief);
void MA_SendRtkBrief(const struct rtkBrief *brief);
void MA_SendCarPosition(const struct carBrief *brief);
-void MA_SendExamWrong(vector<ExamFault> &ExamFaultList);
+void MA_SendExamWrong(std::vector<ExamFault> &ExamFaultList);
void MA_SendRtcmInd(int length);
void MA_SendDebugInfo(const char *str, ...);
void MA_EnterMap(int map_id, int type, int enter);
diff --git a/lib/src/main/cpp/mcu/car_box.cpp b/lib/src/main/cpp/mcu/car_box.cpp
new file mode 100644
index 0000000..f23a4b1
--- /dev/null
+++ b/lib/src/main/cpp/mcu/car_box.cpp
@@ -0,0 +1,112 @@
+//
+// Created by YY on 2024/2/27.
+//
+
+#include "car_box.h"
+#include "../jni_log.h"
+#include "../common/string_util.h"
+#include "../common/serial_port.h"
+#include "../rtk_module/parse_gps.h"
+#include <thread>
+
+#define DEBUG(fmt, args...) LOGD("<car_box> <%s>: " fmt, __func__, ##args)
+
+#define PARSE_BUFF_SIZE 4096
+
+static SerialPort *pCls = nullptr;
+
+static void UartThread(void *p) {
+ struct serial_config *cfg = (struct serial_config *) p;
+
+ pCls = new SerialPort(*cfg);
+
+ int res = pCls->InitSerialPort();
+ DEBUG("Serial %s open %d", cfg->name, res);
+
+ uint8_t RxBuf[PARSE_BUFF_SIZE];
+ int RxBufLen = 0;
+
+ while (res == 0) {
+ int ul = pCls->ReadSerialPort((uint8_t *)RxBuf + RxBufLen, sizeof(RxBuf) - RxBufLen);
+ if (ul < 0) {
+ continue;
+ } else if (ul == 0) {
+ // usb涓插彛鏂紑
+ break;
+ }
+ RxBufLen += ul;
+
+ /*{
+ static char buffd[16384];
+
+ buffd[0] = 0;
+ int i = 0;
+ for (i = 0; i < ul; i++) {
+ if ((i % 32) == 0) {
+ sprintf(buffd + strlen(buffd), "\n");
+ }
+ sprintf(buffd + strlen(buffd), "%02X ", RxBuf[i]);
+ if (strlen(buffd) > 800) {
+ DEBUG("%s <- %s...", "UART", buffd);
+ buffd[0] = 0;
+ }
+ }
+ if (strlen(buffd) > 0)
+ DEBUG("%s <- %s", "UART", buffd);
+ }*/
+
+ if (RxBufLen > 0) {
+ const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
+ if (ptr != RxBuf) {
+ memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));
+ RxBufLen -= ptr - RxBuf;
+ } else if (RxBufLen == sizeof(RxBuf)) { //濉弧浜嗭紝涓旀病鏈変竴涓猏r锛岄兘鎶涘純
+ DEBUG("Parse CarBox error");
+ RxBufLen = 0;
+ }
+ }
+ }
+
+ delete pCls;
+ pCls = nullptr;
+}
+
+void InitCarBox(void)
+{
+ // TODO
+ static struct serial_config serialConfig;
+
+ strcpy(serialConfig.name, "/dev/ttyHSL0");
+ serialConfig.baud = 115200;
+ serialConfig.data_bit = 8;
+ serialConfig.verify_bit = 'N';
+ serialConfig.stop_bit = 1;
+ serialConfig.flow_ctrl = 0;
+
+ std::thread([&] {
+ while (true) {
+ std::thread t(UartThread, &serialConfig);
+ t.join();
+ std::this_thread::sleep_for(std::chrono::seconds(3));
+ }
+ }).detach();
+}
+
+void SendRtcmToUart(const uint8_t *dat, int length) {
+ if (pCls != nullptr) {
+ pCls->WriteSerialPort(dat, length);
+ }
+}
+
+void handleCarinfo(const struct nmea *s) {
+// DEBUG("handleCarinfo");
+}
+
+void handleKsxt(const struct nmea *s) {
+// DEBUG("handleKsxt");
+ char buff[256] = {0};
+
+ memcpy(buff, s->nmea_value[9].data, s->nmea_value[9].length);
+
+ DEBUG("瀹氫綅 %s", buff);
+}
\ No newline at end of file
diff --git a/lib/src/main/cpp/mcu/car_box.h b/lib/src/main/cpp/mcu/car_box.h
new file mode 100644
index 0000000..2976086
--- /dev/null
+++ b/lib/src/main/cpp/mcu/car_box.h
@@ -0,0 +1,13 @@
+//
+// Created by YY on 2024/2/27.
+//
+
+#ifndef MYAPPLICATION3_CAR_BOX_H
+#define MYAPPLICATION3_CAR_BOX_H
+
+#include <cstdint>
+
+void InitCarBox(void);
+void SendRtcmToUart(const uint8_t *dat, int length);
+
+#endif //MYAPPLICATION3_CAR_BOX_H
diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp
index 90f370d..179b288 100644
--- a/lib/src/main/cpp/native-lib.cpp
+++ b/lib/src/main/cpp/native-lib.cpp
@@ -20,6 +20,8 @@
#include "defs.h"
#include "mcu/ada.h"
#include "mcu/ahp.h"
+#include "rtcm.h"
+#include "mcu/car_box.h"
#define DEBUG(fmt, args...) LOGD("<native-lib> <%s>: " fmt, __func__, ##args)
@@ -212,7 +214,7 @@
{
static int seq = 0;
- lock_guard<std::mutex> lock(tts_mutex);
+ std::lock_guard<std::mutex> lock(tts_mutex);
seq++;
return seq;
}
@@ -250,7 +252,7 @@
}
}
- TTSCallBack.insert(pair<int, void (*)(int)>(id, callback));
+ TTSCallBack.insert(std::pair<int, void (*)(int)>(id, callback));
return id;
}
@@ -427,8 +429,9 @@
AppTimer_init();
///////////////ConfigMCU(ayDevice);
- InitAda();
- InitAhp();
+// InitAda();
+// InitAhp();
+ InitCarBox();
DriverTestInit();
MA_Init();
@@ -562,6 +565,17 @@
}
}
+void UploadRtcm(JNIEnv *env, jobject thiz,
+ jbyteArray data, jint length) {
+ // TODO: implement BluetoothDataComeIn()
+ jbyte *c_dat = env->GetByteArrayElements(data, NULL);
+
+ SendRtcmToUart((uint8_t *)c_dat, length);
+// ParseRtcm((uint8_t *)c_dat, length);
+
+ env->ReleaseByteArrayElements(data, c_dat, NULL);
+}
+
static JNINativeMethod methods[] = {
{"startNative", "(Z)V", reinterpret_cast<void *>(startNative)},
{"MainProcMsgEntry", "(ILjava/lang/String;)V", reinterpret_cast<void *>(MainProcMsgEntry)},
@@ -570,7 +584,8 @@
{"TextSpeakEnd", "(I)V", reinterpret_cast<void *>(TextSpeakEnd)},
{"BluetoothConnected", "(Ljava/lang/String;Ljava/lang/String;)V", reinterpret_cast<void *>(BluetoothConnected)},
{"BluetoothStatusChange", "(I)V", reinterpret_cast<void *>(BluetoothStatusChange)},
- {"BluetoothDataComeIn", "([BI)V", reinterpret_cast<void *>(BluetoothDataComeIn)}
+ {"BluetoothDataComeIn", "([BI)V", reinterpret_cast<void *>(BluetoothDataComeIn)},
+ {"uploadRtcm", "([BI)V", reinterpret_cast<void *>(UploadRtcm)}
};
jint JNI_OnLoad(JavaVM *vm, void *reserved)
diff --git a/lib/src/main/cpp/rapidjson/reader.h b/lib/src/main/cpp/rapidjson/reader.h
index 681dec2..6301c40 100644
--- a/lib/src/main/cpp/rapidjson/reader.h
+++ b/lib/src/main/cpp/rapidjson/reader.h
@@ -149,7 +149,7 @@
kParseInsituFlag = 1, //!< In-situ(destructive) parsing.
kParseValidateEncodingFlag = 2, //!< Validate encoding of JSON strings.
kParseIterativeFlag = 4, //!< Iterative(constant complexity in terms of function call stack size) parsing.
- kParseStopWhenDoneFlag = 8, //!< After parsing a complete JSON root from stream, stop further processing the rest of stream. When this flag is used, parser will not generate kParseErrorDocumentRootNotSingular error.
+ kParseStopWhenDoneFlag = 8, //!< After parsing a complete JSON root from stream, stop further processing the rest of stream. When this stopFlag is used, parser will not generate kParseErrorDocumentRootNotSingular error.
kParseFullPrecisionFlag = 16, //!< Parse number in full precision (but slower).
kParseCommentsFlag = 32, //!< Allow one-line (//) and multi-line (/**/) comments.
kParseNumbersAsStringsFlag = 64, //!< Parse all numbers (ints/doubles) as strings.
@@ -2134,7 +2134,7 @@
template <typename InputStream>
void HandleError(IterativeParsingState src, InputStream& is) {
if (HasParseError()) {
- // Error flag has been set.
+ // Error stopFlag has been set.
return;
}
diff --git a/lib/src/main/cpp/rtcm.cpp b/lib/src/main/cpp/rtcm.cpp
new file mode 100644
index 0000000..17d4f39
--- /dev/null
+++ b/lib/src/main/cpp/rtcm.cpp
@@ -0,0 +1,145 @@
+//
+// Created by YY on 2024/2/23.
+//
+
+#include "rtcm.h"
+#include <cstdint>
+#include <cstring>
+#include "jni_log.h"
+#include "native-lib.h"
+#include "mcu/car_box.h"
+
+#define DEBUG(fmt, args...) LOGD("<ntrip-rtcm> <%s>: " fmt, __func__, ##args)
+
+enum parse_rtcm_status_t {
+ SYNC_HEAD,
+ GET_LENGTH_HI,
+ GET_LENGTH_LO,
+ GET_PAYLOAD,
+ GET_CRC_1,
+ GET_CRC_2,
+ GET_CRC_3,
+};
+
+static uint8_t rtcmPackage[1100];
+static int rtcmPayloadLength;
+static int rx_len;
+
+int parse_status = SYNC_HEAD;
+
+static bool CheckCrc(void);
+
+void ParseRtcm(const uint8_t *data, int length) {
+ int x = 0;
+ while (x < length) {
+ uint8_t c = data[x];
+
+ switch (parse_status) {
+ case SYNC_HEAD: {
+ if (data[x] == 0xD3) {
+ parse_status = GET_LENGTH_HI;
+ rtcmPackage[0] = 0xD3;
+ }
+ x++;
+ break;
+ }
+ case GET_LENGTH_HI: {
+ if (data[x] <= 0x03) {
+ parse_status = GET_LENGTH_LO;
+ rtcmPayloadLength = data[x];
+ rtcmPackage[1] = data[x];
+ } else {
+ parse_status = SYNC_HEAD;
+ }
+ x++;
+ break;
+ }
+ case GET_LENGTH_LO: {
+ rtcmPayloadLength = (rtcmPayloadLength << 8) + data[x];
+ parse_status = GET_PAYLOAD;
+ rtcmPackage[2] = data[x];
+ rx_len = 0;
+ x++;
+ break;
+ }
+ case GET_PAYLOAD: {
+ if (length - x >= rtcmPayloadLength - rx_len) {
+ memcpy(rtcmPackage + 3 + rx_len, data + x, rtcmPayloadLength - rx_len);
+ x += rtcmPayloadLength - rx_len;
+ rx_len = rtcmPayloadLength;
+ parse_status = GET_CRC_1;
+ } else {
+ memcpy(rtcmPackage + 3 + rx_len, data + x, length - x);
+ rx_len += length - x;
+ x = length;
+ }
+ break;
+ }
+ case GET_CRC_1: {
+ rtcmPackage[3+rtcmPayloadLength] = c;
+ parse_status = GET_CRC_2;
+ x++;
+ break;
+ }
+ case GET_CRC_2: {
+ rtcmPackage[3+rtcmPayloadLength+1] = c;
+ parse_status = GET_CRC_3;
+ x++;
+ break;
+ }
+ case GET_CRC_3: {
+ rtcmPackage[3+rtcmPayloadLength+2] = c;
+
+ if (CheckCrc()) {
+ SendRtcmToUart(rtcmPackage, 6 + rtcmPayloadLength);
+ }
+ parse_status = SYNC_HEAD;
+ x++;
+ break;
+ }
+ }
+
+ }
+}
+
+static bool CheckCrc(void) {
+ static const int table[]={
+ 0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17,
+ 0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E, 0xC54E89, 0x430272,
+ 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E, 0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646,
+ 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, 0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3,
+ 0x15E205, 0x93AEFE, 0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7,
+ 0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077, 0x681E59, 0xEE52A2,
+ 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E, 0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24,
+ 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5, 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1,
+ 0xA11107, 0x275DFC, 0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C,
+ 0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, 0x15723B, 0x933EC0,
+ 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, 0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4,
+ 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15, 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8,
+ 0xC90F5E, 0x4F43A5, 0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C,
+ 0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3, 0x92C69D, 0x148A66,
+ 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A, 0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB,
+ 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, 0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E,
+ 0x4EBBF8, 0xC8F703, 0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A,
+ 0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863, 0xFAD8C4, 0x7C943F,
+ 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3, 0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B,
+ 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA, 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C,
+ 0x33D79A, 0xB59B61, 0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58,
+ 0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, 0x4E2BC6, 0xC8673D,
+ 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1, 0x26359F, 0xA07964, 0xACE092, 0x2AAC69,
+ 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88, 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC,
+ 0x9E874A, 0x18CBB1, 0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401,
+ 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538};
+
+ int crc = 0;
+ for(int i = 0; i< 3 + rtcmPayloadLength; i++) {
+ crc = ((crc << 8) & 0xFFFFFF) ^ table[(crc >> 16) ^ (rtcmPackage[i] & 0xff)];
+ }
+ if (crc == (rtcmPackage[3+rtcmPayloadLength]<<16) + (rtcmPackage[3+rtcmPayloadLength + 1]<<8) + rtcmPackage[3+rtcmPayloadLength + 2]) {
+ return true;
+ }
+
+ DEBUG("鏍￠獙澶辫触 %d: %X - %X", crc, crc, (rtcmPackage[3+rtcmPayloadLength]<<16) + (rtcmPackage[3+rtcmPayloadLength + 1]<<8) + rtcmPackage[3+rtcmPayloadLength + 2]);
+
+ return false;
+}
\ No newline at end of file
diff --git a/lib/src/main/cpp/rtcm.h b/lib/src/main/cpp/rtcm.h
new file mode 100644
index 0000000..82230a1
--- /dev/null
+++ b/lib/src/main/cpp/rtcm.h
@@ -0,0 +1,12 @@
+//
+// Created by YY on 2024/2/23.
+//
+
+#ifndef MYAPPLICATION3_RTCM_H
+#define MYAPPLICATION3_RTCM_H
+
+#include <cstdint>
+
+void ParseRtcm(const uint8_t *data, int length);
+
+#endif //MYAPPLICATION3_RTCM_H
diff --git a/lib/src/main/cpp/rtk_module/parse_gps.cpp b/lib/src/main/cpp/rtk_module/parse_gps.cpp
index b81dab3..390c149 100644
--- a/lib/src/main/cpp/rtk_module/parse_gps.cpp
+++ b/lib/src/main/cpp/rtk_module/parse_gps.cpp
@@ -267,6 +267,14 @@
handleRTKRebootComp(&nmeas);
continue;
}
+ if(match2(getHexNumber(matchChar(getNMEA(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(start = matchChar(skip1(s, e), e, '$'), e, 'C'), e, 'A'), e, 'R'), e, 'I'), e, 'N'), e, 'F'), e, 'O'), e, &nmeas), e, '*'), e, start), e)) {
+ handleCarinfo(&nmeas);
+ continue;
+ }
+ if(match2(getHexNumber(matchChar(getNMEA(matchChar(matchChar(matchChar(matchChar(start = matchChar(skip1(s, e), e, '$'), e, 'K'), e, 'S'), e, 'X'), e, 'T'), e, &nmeas), e, '*'), e, start), e)) {
+ handleKsxt(&nmeas);
+ continue;
+ }
handleUnrecognisedNMEA(s, p-s);
}
}
diff --git a/lib/src/main/cpp/rtk_module/parse_gps.h b/lib/src/main/cpp/rtk_module/parse_gps.h
index 7064785..c8123a5 100644
--- a/lib/src/main/cpp/rtk_module/parse_gps.h
+++ b/lib/src/main/cpp/rtk_module/parse_gps.h
@@ -35,5 +35,7 @@
void handleBESTPOSA(const struct nmea *);
void handlePJKParam(const struct nmea *s);
void handleRTKRebootComp(const struct nmea *s);
+void handleCarinfo(const struct nmea *s);
+void handleKsxt(const struct nmea *s);
#endif
diff --git a/lib/src/main/cpp/rtk_module/rtk.cpp b/lib/src/main/cpp/rtk_module/rtk.cpp
index de85611..069d630 100644
--- a/lib/src/main/cpp/rtk_module/rtk.cpp
+++ b/lib/src/main/cpp/rtk_module/rtk.cpp
@@ -160,16 +160,16 @@
DEBUG("handleUnrecognisedNMEA: %s", buff);
if (length >= 100) {
- string cs(buff);
+ std::string cs(buff);
- if (cs.find("K708") != string::npos) {
+ if (cs.find("K708") != std::string::npos) {
// 鏈�鍒濈殑鏍囧噯鍩虹珯妯″潡
DEBUG("K708 妯″潡");
strcpy(rtkModel, "K708");
AppTimer_delete(VersionTimeout);
CheckPjkParam();
- } else if (cs.find("K726") != string::npos) {
+ } else if (cs.find("K726") != std::string::npos) {
// 绉诲姩绔欐ā鍧楋紝涔熷彲浠ョ敤鍋氬熀绔欏姛鑳�
DEBUG("K726 妯″潡");
strcpy(rtkModel, "K726");
diff --git a/lib/src/main/cpp/rtk_module/virtual_rtk.cpp b/lib/src/main/cpp/rtk_module/virtual_rtk.cpp
index f205d6b..d193cdc 100644
--- a/lib/src/main/cpp/rtk_module/virtual_rtk.cpp
+++ b/lib/src/main/cpp/rtk_module/virtual_rtk.cpp
@@ -12,6 +12,7 @@
#include "../defs.h"
#include "parse_gps.h"
#include "../mcu/mcu_if.h"
+#include "../rtk_platform/platform.h"
#define DEBUG(fmt, args...) LOGD("<virtual_device> <%s>: " fmt, __func__, ##args)
@@ -35,6 +36,10 @@
static void ConnectLater(apptimer_var_t val);
static void ConnectLater2(apptimer_var_t val);
+
+static void VirtualCommondEntry(uint16_t id, const uint8_t *data, int length);
+
+static ParseUart parse(VirtualCommondEntry);
void InitVirtualDevice(const char *domain_name, int port)
{
@@ -175,11 +180,25 @@
if (RxBufLen > 0) {
/////////////////ParseMcu(RxBuf, RxBufLen);
+ parse.ParseMcu(RxBuf, RxBufLen);
RxBufLen = 0;
}
}
}
+static void VirtualCommondEntry(uint16_t id, const uint8_t *data, int length)
+{
+ DEBUG("鏀跺埌鍛戒护 0x%04X", id);
+ switch (id) {
+ case 0x8006:
+ PlatformStatusChanged(CAR_SENSOR_UPDATE_EVT, data, length);
+ break;
+ case 0x8007:
+ PlatformStatusChanged(MCU_UPDATE_EVT, data, length);
+ break;
+ }
+}
+
/*
static void *VDataListenThread(void *p) {
struct vSocket *vs = (struct vSocket *)p;
diff --git a/lib/src/main/cpp/rtk_platform/platform.cpp b/lib/src/main/cpp/rtk_platform/platform.cpp
index a48d7d2..89d0331 100644
--- a/lib/src/main/cpp/rtk_platform/platform.cpp
+++ b/lib/src/main/cpp/rtk_platform/platform.cpp
@@ -399,7 +399,7 @@
case GPS_UPDATE_EVT: {
const gpsStatus_t *gps = (gpsStatus_t *) data;
-// DEBUG("GPS: %s", const_cast<gpsStatus_t *>(gps)->toString().c_str());
+ DEBUG("GPS: %s", const_cast<gpsStatus_t *>(gps)->toString().c_str());
gbf.qf = gps->gps_status;
gbf.latitude = gps->latitude;
@@ -431,7 +431,7 @@
const rtk_info_t *rtk = (rtk_info_t *) data;
-// DEBUG("RTK: %s", const_cast<rtk_info_t *>(rtk)->toString().c_str());
+ DEBUG("RTK: %s", const_cast<rtk_info_t *>(rtk)->toString().c_str());
rbf.qf = rtk->qf;
rbf.coord_x = rtk->y;
@@ -516,7 +516,7 @@
}
defaultMcuRom.more = 0;
}
-// UpdateSensor(brief.gpio, brief.speed, brief.engine);
+// UpdateSensorHw(brief.gpio, brief.speed, brief.engine);
break;
}
case CAN_UPDATE_EVT: {
@@ -602,7 +602,7 @@
// if (sensor.clutch == 1)
// sensor.gear = 0;
- UpdateSensor(&sensor);
+ UpdateSensorHw(&sensor);
break;
}
case CARD_UPDATE_EVT: {
@@ -711,7 +711,10 @@
break;
}
case SENSOR_CHANGE_EVT: {
- SensorXChanged(BUILD_UINT16(data[1], data[0]), BUILD_UINT32(data[5], data[4], data[3], data[2]));
+ // 杞﹁締浠〃鐘舵�佸彂鐢熷彉鍖栵紝闄や簡杞﹂�熷拰杞��
+ int id = BUILD_UINT16(data[1], data[0]);
+ int value = BUILD_UINT32(data[5], data[4], data[3], data[2]);
+ UpdateSensor(id, value);
break;
}
default:
diff --git a/lib/src/main/cpp/teach/teach.cpp b/lib/src/main/cpp/teach/teach.cpp
new file mode 100644
index 0000000..73ecb67
--- /dev/null
+++ b/lib/src/main/cpp/teach/teach.cpp
@@ -0,0 +1,209 @@
+//
+// Created by YY on 2023/4/20.
+//
+
+#include "teach.h"
+#include "../jni_log.h"
+#include "../driver_test.h"
+#include "../native-lib.h"
+#include <tuple>
+#include <vector>
+#include <algorithm>
+
+#define DEBUG(fmt, args...) LOGD("<teach> <%s>: " fmt, __func__, ##args)
+
+typedef struct {
+ int id;
+ std::string tts;
+ int stage;
+ int refer; // 杞﹁韩鏌愮偣锛屾煇鏌愪簨浠�
+ PointF point; // 閲囬泦鐐圭粡杩囧潗鏍囧彉鎹㈠悗寰楀埌
+} point_tip_t;
+
+typedef struct {
+ int id;
+ std::string tts;
+ int stage;
+ int refer; // 杞﹁韩鏌愮偣锛屾煇鏌愪簨浠�
+ double distance;
+ Line line; // 閲囬泦鐐圭粡杩囧潗鏍囧彉鎹㈠悗寰楀埌
+} line_tip_t;
+
+typedef struct {
+ int item;
+ std::vector<point_tip_t> tips;
+} teach_tips_t;
+
+typedef struct {
+
+};
+
+teach_tips_t all;
+
+void testAll(void)
+{
+ all.item = MAP_TYPE_PARK_BUTTOM;
+
+ point_tip_t p1 = {
+ .id = 1,
+ .tts = "鎻愮ず涓�",
+ .stage = 2,
+ .refer = 1,
+ .point = {.X = 4.396647, .Y = 4.967977}
+ };
+
+ point_tip_t p2 = {
+ .id = 2,
+ .tts = "鎻愮ず浜�",
+ .stage = 2,
+ .refer = 1,
+ .point = {.X = 1.352043, .Y = 3.434146}
+ };
+
+ point_tip_t p3 = {
+ .id = 3,
+ .tts = "鎻愮ず涓�",
+ .stage = 2,
+ .refer = 1,
+ .point = {.X = 1.042866, .Y = 0.124397}
+ };
+
+ point_tip_t p4 = {
+ .id = 4,
+ .tts = "鎻愮ず鍥�",
+ .stage = 2,
+ .refer = 1,
+ .point = {.X = 1.037644, .Y = -4.675115}
+ };
+
+ all.tips.push_back(p1);
+ all.tips.push_back(p2);
+ all.tips.push_back(p3);
+ all.tips.push_back(p4);
+}
+
+class Foobar {
+public:
+ int id;
+ double distance;
+
+ Foobar(int id, double distance): id(id), distance(distance) {}
+ ~Foobar() = default;
+
+ bool operator<(const Foobar &bar)
+ {
+ if (this->distance < bar.distance) {
+ return true;
+ }
+ return false;
+ }
+
+ bool static decrease(const Foobar &foo1, const Foobar &foo2)
+ {
+ if (foo1.distance > foo2.distance)
+ {
+ return false;
+ }
+ return true;
+ }
+
+};
+
+std::vector<Foobar> status;
+
+// 褰撳墠椤圭洰闃舵鍙戠敓鍙樺寲鏃讹紝浜х敓鐨勭偣浣嶆彁绀�
+void LoadStageTips(prime_t &prime)
+{
+ DEBUG("LoadStageTips %d", prime.examing_area.stage);
+
+ std::vector<Foobar>().swap(status);
+
+ // 浠�2鍙风偣(宸︿晶杞﹀簱鍏ュ彛瑙�)涓哄師鐐�
+ double dx = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2].X;
+ double dy = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2].Y;
+
+ // 浠�2---->3绾夸负-Y杞达紝鏃嬭浆
+ double angle = YawOf(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3]);
+
+ if (angle > 180) {
+ angle = angle - 180;
+ } else {
+ angle = angle + 180;
+ }
+
+ // 璁板綍杞﹁締浣嶇疆缁忚繃鍧愭爣鍙樻崲鍚庣殑鍊�
+ PointF car = {.X = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]].X - dx,
+ .Y = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]].Y - dy};
+
+ car = rotatePoint(car, {.X = 0,.Y = 0}, angle);
+
+ // 闇�鎸夌収璺濈杩滆繎閫掑鎺掑簭
+ for (int i = 0; i < all.tips.size(); ++i) {
+ if (all.tips[i].stage == prime.examing_area.stage) {
+ DEBUG("Add %d: %f", i, DistanceOf(car, all.tips[i].point));
+ status.push_back(Foobar(i, DistanceOf(car, all.tips[i].point)));
+ }
+ }
+ if (status.size() > 0) {
+ std::sort(status.begin(), status.end(), Foobar::decrease);
+
+ for (auto st: status) {
+ DEBUG("%d: %f", st.id, st.distance);
+ }
+ }
+}
+
+void teach(prime_t &prime)
+{
+ if(status.size() == 0)
+ return;
+
+ PointF car;
+ PointF &teach_point = all.tips[status.front().id].point;
+
+ switch (prime.examing_area.type) {
+ case MAP_TYPE_PARK_BUTTOM: {
+ // 浠�2鍙风偣(宸︿晶杞﹀簱鍏ュ彛瑙�)涓哄師鐐�
+ double dx = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2].X;
+ double dy = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2].Y;
+
+ // 浠�2---->3绾夸负-Y杞达紝鏃嬭浆
+ double angle = YawOf(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3]);
+
+ if (angle > 180) {
+ angle = angle - 180;
+ } else {
+ angle = angle + 180;
+ }
+
+ // 璁板綍杞﹁締浣嶇疆缁忚繃鍧愭爣鍙樻崲鍚庣殑鍊�
+ car = {.X = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]].X - dx,
+ .Y = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]].Y - dy};
+
+ car = rotatePoint(car, {.X = 0,.Y = 0}, angle);
+
+ break;
+ }
+ case MAP_TYPE_PARK_EDGE:
+ break;
+ case MAP_TYPE_RIGHT_CORNER:
+ break;
+ case MAP_TYPE_UPHILL:
+ break;
+ case MAP_TYPE_CURVE:
+ break;
+ default:break;
+ }
+
+ double dis = DistanceOf(car, teach_point);
+
+ DEBUG("XY(%f,%f, dis = %f)", car.X, car.Y, dis);
+
+ if (dis < status.front().distance) {
+ status.front().distance = dis;
+ } else if (dis > status.front().distance && dis - status.front().distance > 0.05) {
+ // 鎻愮ず
+ PlayTTS(all.tips[status.front().id].tts, nullptr);
+ status.erase(status.begin());
+ }
+}
diff --git a/lib/src/main/cpp/teach/teach.h b/lib/src/main/cpp/teach/teach.h
new file mode 100644
index 0000000..8f442a5
--- /dev/null
+++ b/lib/src/main/cpp/teach/teach.h
@@ -0,0 +1,14 @@
+//
+// Created by YY on 2023/4/20.
+//
+
+#ifndef MYAPPLICATION3_TEACH_H
+#define MYAPPLICATION3_TEACH_H
+
+#include "../driver_test.h"
+
+void testAll(void);
+void LoadStageTips(prime_t &prime);
+void teach(prime_t &prime);
+
+#endif //MYAPPLICATION3_TEACH_H
diff --git a/lib/src/main/cpp/teach/train.cpp b/lib/src/main/cpp/teach/train.cpp
new file mode 100644
index 0000000..d43e5fd
--- /dev/null
+++ b/lib/src/main/cpp/teach/train.cpp
@@ -0,0 +1,51 @@
+//
+// Created by YY on 2023/4/20.
+//
+
+#include <tuple>
+#include "train.h"
+#include "../driver_test.h"
+#include "../jni_log.h"
+
+#define DEBUG(fmt, args...) LOGD("<train> <%s>: " fmt, __func__, ##args)
+
+// 寰楀埌鐐瑰悗锛岀浉搴斿満鍦扮殑鍧愭爣鍙樻崲
+// coordinate transformation
+void train(prime_t &prime)
+{
+ park_button_map_t az;
+
+ az.points.resize(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points.size());
+
+ // 浠�2鍙风偣(宸︿晶杞﹀簱鍏ュ彛瑙�)涓哄師鐐�
+ double dx = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2].X;
+ double dy = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2].Y;
+
+ for (int i = 0; i < std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points.size(); ++i) {
+ az.points[i] = {.X = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[i].X - dx,
+ .Y = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[i].Y - dy};
+ }
+
+ // 浠�2---->3绾夸负-Y杞达紝鏃嬭浆
+ double angle = YawOf(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3]);
+
+ if (angle > 180) {
+ angle = angle - 180;
+ } else {
+ angle = angle + 180;
+ }
+
+ for (int i = 0; i < az.points.size(); ++i) {
+ if (i != 2) {
+ az.points[i] = rotatePoint(az.points[i], az.points[2], angle);
+ }
+ }
+
+ // 璁板綍杞﹁締浣嶇疆缁忚繃鍧愭爣鍙樻崲鍚庣殑鍊�
+ PointF car = {.X = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_REAR]].X - dx,
+ .Y = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_REAR]].Y - dy};
+
+ car = rotatePoint(car, az.points[2], angle);
+
+ DEBUG("鍧愭爣鍙樻崲鍚庣殑 %f,%f", car.X, car.Y);
+}
diff --git a/lib/src/main/cpp/teach/train.h b/lib/src/main/cpp/teach/train.h
new file mode 100644
index 0000000..b6e6bb5
--- /dev/null
+++ b/lib/src/main/cpp/teach/train.h
@@ -0,0 +1,12 @@
+//
+// Created by YY on 2023/4/20.
+//
+
+#ifndef MYAPPLICATION3_TRAIN_H
+#define MYAPPLICATION3_TRAIN_H
+
+#include "../driver_test.h"
+
+void train(prime_t &prime);
+
+#endif //MYAPPLICATION3_TRAIN_H
diff --git a/lib/src/main/cpp/test_common/Geometry.cpp b/lib/src/main/cpp/test_common/Geometry.cpp
index 8fe70d1..c6bd694 100644
--- a/lib/src/main/cpp/test_common/Geometry.cpp
+++ b/lib/src/main/cpp/test_common/Geometry.cpp
@@ -43,15 +43,13 @@
return (radians * 180.0) / M_PI;
}
-void MakeLine(Line *line, const PointF *p1, const PointF *p2)
+void MAKE_LINE(Line &a, PointF &b, PointF &c)
{
- line->X1 = p1->X;
- line->Y1 = p1->Y;
- line->X2 = p2->X;
- line->Y2 = p2->Y;
+ a.p1 = b;
+ a.p2 = c;
}
-void MakePolygon(Polygon *polygon, std::initializer_list<PointF> point_set)
+/*void MakePolygon(Polygon *polygon, std::initializer_list<PointF> point_set)
{
int n = 0;
@@ -61,12 +59,12 @@
polygon->point[n++] = ptr;
}
polygon->num = n;
-}
+}*/
void MakeHidePoint(PointF *point, const PointF *bp, const Line *bl)
{
- point->X = (bl->X1 + bl->X2) - bp->X;
- point->Y = (bl->Y1 + bl->Y2) - bp->Y;
+ point->X = (bl->p1.X + bl->p2.X) - bp->X;
+ point->Y = (bl->p1.Y + bl->p2.Y) - bp->Y;
}
void CleanPolygon(Polygon *polygon)
@@ -127,13 +125,13 @@
int index2 = (index + 1) % polygon->num;
Line line2;
- line2.X1 = polygon->point[index].X;
- line2.Y1 = polygon->point[index].Y;
- line2.X2 = polygon->point[index2].X;
- line2.Y2 = polygon->point[index2].Y;
+ line2.p1.X = polygon->point[index].X;
+ line2.p1.Y = polygon->point[index].Y;
+ line2.p2.X = polygon->point[index2].X;
+ line2.p2.Y = polygon->point[index2].Y;
-// LOGD("line1(%d %d - %d %d) line2(%d %d - %d %d)", line.X1, line.Y1, line.X2, line.Y2,
-// line2.X1, line2.Y1, line2.X2, line2.Y2);
+// LOGD("line1(%d %d - %d %d) line2(%d %d - %d %d)", line.p1.X, line.p1.Y, line.p2.X, line.p2.Y,
+// line2.p1.X, line2.p1.Y, line2.p2.X, line2.p2.Y);
relation_t relation = IntersectionOf(line, line2);
@@ -149,8 +147,8 @@
PointF point2;
- point2.X = line.X1;
- point2.Y = line.Y1;
+ point2.X = line.p1.X;
+ point2.Y = line.p1.Y;
return tangent ? GM_Tangent : IntersectionOf(point2, polygon);
}
@@ -172,10 +170,10 @@
case 2: {
Line line2;
- line2.X1 = polygon->point[0].X;
- line2.Y1 = polygon->point[0].Y;
- line2.X2 = polygon->point[1].X;
- line2.Y2 = polygon->point[1].Y;
+ line2.p1.X = polygon->point[0].X;
+ line2.p1.Y = polygon->point[0].Y;
+ line2.p2.X = polygon->point[1].X;
+ line2.p2.Y = polygon->point[1].Y;
return IntersectionOf(point, line2);
}
default:
@@ -224,21 +222,21 @@
relation_t IntersectionOf(PointF point, Line line)
{
- double bottomY = fmin(line.Y1, line.Y2);
- double topY = fmax(line.Y1, line.Y2);
+ double bottomY = fmin(line.p1.Y, line.p2.Y);
+ double topY = fmax(line.p1.Y, line.p2.Y);
bool heightIsRight = point.Y >= bottomY &&
point.Y <= topY;
//Vertical line, slope is divideByZero error!
- if (isEqual(line.X1, line.X2)) {
- if (isEqual(point.X, line.X1) && heightIsRight) {
+ if (isEqual(line.p1.X, line.p2.X)) {
+ if (isEqual(point.X, line.p1.X) && heightIsRight) {
return GM_Tangent;
} else {
return GM_None;
}
}
- double slope = (line.X2 - line.X1) / (line.Y2 - line.Y1);
- bool onLine = isEqual(line.Y1 - point.Y, slope * (line.X1 - point.X));
+ double slope = (line.p2.X - line.p1.X) / (line.p2.Y - line.p1.Y);
+ bool onLine = isEqual(line.p1.Y - point.Y, slope * (line.p1.X - point.X));
if (onLine && heightIsRight) {
return GM_Tangent;
@@ -250,41 +248,41 @@
relation_t IntersectionOf(Line line1, Line line2)
{
// Fail if either line segment is zero-length.
- if ((isEqual(line1.X1, line1.X2) && isEqual(line1.Y1, line1.Y2)) || (isEqual(line2.X1, line2.X2) && isEqual(line2.Y1, line2.Y2)))
+ if ((isEqual(line1.p1.X, line1.p2.X) && isEqual(line1.p1.Y, line1.p2.Y)) || (isEqual(line2.p1.X, line2.p2.X) && isEqual(line2.p1.Y, line2.p2.Y)))
return GM_None;
- if ((isEqual(line1.X1, line2.X1) && isEqual(line1.Y1, line2.Y1)) || (isEqual(line1.X2, line2.X1) && isEqual(line1.Y2, line2.Y1)))
+ if ((isEqual(line1.p1.X, line2.p1.X) && isEqual(line1.p1.Y, line2.p1.Y)) || (isEqual(line1.p2.X, line2.p1.X) && isEqual(line1.p2.Y, line2.p1.Y)))
return GM_Intersection;
- if ((isEqual(line1.X1, line2.X2) && isEqual(line1.Y1, line2.Y2)) || (isEqual(line1.X2, line2.X2) && isEqual(line1.Y2, line2.Y2)))
+ if ((isEqual(line1.p1.X, line2.p2.X) && isEqual(line1.p1.Y, line2.p2.Y)) || (isEqual(line1.p2.X, line2.p2.X) && isEqual(line1.p2.Y, line2.p2.Y)))
return GM_Intersection;
// (1) Translate the system so that point A is on the origin.
- line1.X2 -= line1.X1; line1.Y2 -= line1.Y1;
- line2.X1 -= line1.X1; line2.Y1 -= line1.Y1;
- line2.X2 -= line1.X1; line2.Y2 -= line1.Y1;
+ line1.p2.X -= line1.p1.X; line1.p2.Y -= line1.p1.Y;
+ line2.p1.X -= line1.p1.X; line2.p1.Y -= line1.p1.Y;
+ line2.p2.X -= line1.p1.X; line2.p2.Y -= line1.p1.Y;
// Discover the length of segment A-B.
- double distAB = sqrt(line1.X2 * line1.X2 + line1.Y2 * line1.Y2);
+ double distAB = sqrt(line1.p2.X * line1.p2.X + line1.p2.Y * line1.p2.Y);
// (2) Rotate the system so that point B is on the positive X axis.
- double theCos = line1.X2 / distAB;
- double theSin = line1.Y2 / distAB;
- double newX = line2.X1 * theCos + line2.Y1 * theSin;
+ double theCos = line1.p2.X / distAB;
+ double theSin = line1.p2.Y / distAB;
+ double newX = line2.p1.X * theCos + line2.p1.Y * theSin;
- line2.Y1 = line2.Y1 * theCos - line2.X1 * theSin;
- line2.X1 = newX;
- newX = line2.X2 * theCos + line2.Y2 * theSin;
- line2.Y2 = line2.Y2 * theCos - line2.X2 * theSin;
- line2.X2 = newX;
+ line2.p1.Y = line2.p1.Y * theCos - line2.p1.X * theSin;
+ line2.p1.X = newX;
+ newX = line2.p2.X * theCos + line2.p2.Y * theSin;
+ line2.p2.Y = line2.p2.Y * theCos - line2.p2.X * theSin;
+ line2.p2.X = newX;
// Fail if segment C-D doesn't cross line A-B.
- if ((line2.Y1 < 0 && line2.Y2 < 0) || (line2.Y1 >= 0 && line2.Y2 >= 0)) {
+ if ((line2.p1.Y < 0 && line2.p2.Y < 0) || (line2.p1.Y >= 0 && line2.p2.Y >= 0)) {
return GM_None;
}
// (3) Discover the position of the intersection point along line A-B.
- double posAB = line2.X2 + (line2.X1 - line2.X2) * line2.Y2 / (line2.Y2 - line2.Y1);
+ double posAB = line2.p2.X + (line2.p1.X - line2.p2.X) * line2.p2.Y / (line2.p2.Y - line2.p1.Y);
// Fail if segment C-D crosses line A-B outside of segment A-B.
if (posAB < 0 || posAB > distAB) {
@@ -301,19 +299,19 @@
double DistanceOf(PointF point, Line line)
{
-// float a = sqrt((point.X-line.X1)*(point.X-line.X1) + (point.Y-line.Y1)*(point.Y-line.Y1));
-// float b = sqrt((point.X-line.X2)*(point.X-line.X2) + (point.Y-line.Y2)*(point.Y-line.Y2));
- double c = sqrt((line.X1-line.X2)*(line.X1-line.X2) + (line.Y1-line.Y2)*(line.Y1-line.Y2));
+// float a = sqrt((point.X-line.p1.X)*(point.X-line.p1.X) + (point.Y-line.p1.Y)*(point.Y-line.p1.Y));
+// float b = sqrt((point.X-line.p2.X)*(point.X-line.p2.X) + (point.Y-line.p2.Y)*(point.Y-line.p2.Y));
+ double c = sqrt((line.p1.X-line.p2.X)*(line.p1.X-line.p2.X) + (line.p1.Y-line.p2.Y)*(line.p1.Y-line.p2.Y));
// float p = (a+b+c)/2;
// dis = 2 * sqrt(p*(p-a)*(p-b)*(p-c)) / c;
- return fabs(point.X*line.Y1 + point.Y*line.X2 + line.X1*line.Y2 - line.X2*line.Y1 - line.X1*point.Y - point.X*line.Y2) / c;
+ return fabs(point.X*line.p1.Y + point.Y*line.p2.X + line.p1.X*line.p2.Y - line.p2.X*line.p1.Y - line.p1.X*point.Y - point.X*line.p2.Y) / c;
}
/*********************************************************
- * p1----------->p2 绾跨鍜孻杞寸殑澶硅
+ * p1----------->p2 绾跨鍜孻杞寸殑澶硅(椤烘椂閽�)
* @param p1
* @param p2
* @return yaw
@@ -361,13 +359,13 @@
double YawOf(Line &line)
{
PointF p1 = {
- .X = line.X1,
- .Y = line.Y1
+ .X = line.p1.X,
+ .Y = line.p1.Y
};
PointF p2 = {
- .X = line.X2,
- .Y = line.Y2
+ .X = line.p2.X,
+ .Y = line.p2.Y
};
return YawOf(p1, p2);
@@ -383,20 +381,20 @@
*/
double AngleOf(PointF p1, PointF p2, PointF p3)
{
- int rel = IntersectionOfLine(p1, p2, p3);
+ relational_position_t rel = IntersectionOfLine(p1, p2, p3);
switch (rel) {
- case RELATION_LEFT:
- case RELATION_RIGHT: {
+ case REL_POS_LEFT:
+ case REL_POS_RIGHT: {
double a = DistanceOf(p2, p3);
double b = DistanceOf(p1, p2);
double c = DistanceOf(p1, p3);
double deg = toDegree(acos((pow(b, 2) + pow(c, 2) - pow(a, 2)) / (2 * b * c)));
- return (rel == RELATION_LEFT) ? deg : (360-deg);
+ return (rel == REL_POS_LEFT) ? deg : (360-deg);
}
- case RELATION_BACK:
+ case REL_POS_REAR:
return 180;
default:
return 0;
@@ -413,9 +411,9 @@
*/
double AngleOfTowLine(Line base, Line dest)
{
- PointF p1 = {.X = base.X1, .Y = base.Y1};
- PointF p2 = {.X = base.X2, .Y = base.Y2};
- PointF p3 = {.X = dest.X2 + (base.X1 - dest.X1), .Y = dest.Y2 + (base.Y1 - dest.Y1)};
+ PointF p1 = {.X = base.p1.X, .Y = base.p1.Y};
+ PointF p2 = {.X = base.p2.X, .Y = base.p2.Y};
+ PointF p3 = {.X = dest.p2.X + (base.p1.X - dest.p1.X), .Y = dest.p2.Y + (base.p1.Y - dest.p1.Y)};
return AngleOf(p1, p2, p3);
}
@@ -443,15 +441,15 @@
{
double angle = 0;
- double dx = base.X2 - dest.X2;
- double dy = base.Y2 - dest.Y2;
+ double dx = base.p2.X - dest.p2.X;
+ double dy = base.p2.Y - dest.p2.Y;
- dest.X1 += dx;
- dest.Y1 += dy;
+ dest.p1.X += dx;
+ dest.p1.Y += dy;
- double c2 = pow((dest.X1 - base.X1), 2) + pow((dest.Y1 - base.Y1), 2);
- double a2 = pow((base.X1 - base.X2), 2) + pow((base.Y1 - base.Y2), 2);
- double b2 = pow((dest.X1 - base.X2), 2) + pow((dest.Y1 - base.Y2), 2);
+ double c2 = pow((dest.p1.X - base.p1.X), 2) + pow((dest.p1.Y - base.p1.Y), 2);
+ double a2 = pow((base.p1.X - base.p2.X), 2) + pow((base.p1.Y - base.p2.Y), 2);
+ double b2 = pow((dest.p1.X - base.p2.X), 2) + pow((dest.p1.Y - base.p2.Y), 2);
angle = acos((a2 + b2 - c2) / (2 * sqrt(a2) * sqrt(b2)));
@@ -535,10 +533,10 @@
{
PointF p1, p2;
- p1.X = line.X1;
- p1.Y = line.Y1;
- p2.X = line.X2;
- p2.Y = line.Y2;
+ p1.X = line.p1.X;
+ p1.Y = line.p1.Y;
+ p2.X = line.p2.X;
+ p2.Y = line.p2.Y;
return IntersectionOfLine(p1, p2, p);
}
@@ -588,11 +586,11 @@
{
PointF p1, p2;
- p1.X = line.X1;
- p1.Y = line.Y1;
+ p1.X = line.p1.X;
+ p1.Y = line.p1.Y;
- p2.X = line.X2;
- p2.Y = line.Y2;
+ p2.X = line.p2.X;
+ p2.Y = line.p2.Y;
PointF pv = GetVerticalPoint(p1, p2, point);
@@ -607,11 +605,11 @@
{
PointF p1, p2;
- p1.X = line.X1;
- p1.Y = line.Y1;
+ p1.X = line.p1.X;
+ p1.Y = line.p1.Y;
- p2.X = line.X2;
- p2.Y = line.Y2;
+ p2.X = line.p2.X;
+ p2.Y = line.p2.Y;
PointF pv = GetVerticalPoint(p1, p2, point);
vp = pv;
@@ -625,7 +623,7 @@
/****************************************************************
* p3
- * | 'L'
+ * | 'length'
* |
* p1------------------>p2
* |
@@ -633,11 +631,11 @@
* p3
* @param p1
* @param p2
- * @param L
+ * @param length
* @param dir
* @return
*/
-PointF Calc3Point(PointF p1, PointF p2, double L, char dir)
+PointF Calc3Point(PointF p1, PointF p2, double length, char dir)
{
PointF p3;
@@ -649,18 +647,18 @@
if (isEqual(p1.X, p2.X)) {
p3.Y = p2.Y;
if (p2.Y > p1.Y) {
- p3.X = p2.X + ((dir == 'L')? -L:L);
+ p3.X = p2.X + ((dir == 'L') ? -length : length);
} else {
- p3.X = p2.X + ((dir=='L')?L:-L);
+ p3.X = p2.X + ((dir=='L') ? length : -length);
}
return p3;
}
if (isEqual(p1.Y, p2.Y)) {
p3.X = p2.X;
if (p2.X > p1.X) {
- p3.Y = p2.Y + ((dir == 'L')? L:-L);
+ p3.Y = p2.Y + ((dir == 'L') ? length : -length);
} else {
- p3.Y = p2.Y + ((dir == 'L')? -L:L);
+ p3.Y = p2.Y + ((dir == 'L') ? -length : length);
}
return p3;
}
@@ -670,7 +668,7 @@
double A = 1 + pow(k, 2);
double B = 2*k*(b - p2.Y) - 2*p2.X;
- double C = pow(b - p2.Y, 2) + pow(p2.X, 2) - pow(L,2);
+ double C = pow(b - p2.Y, 2) + pow(p2.X, 2) - pow(length, 2);
double x3, y3;
diff --git a/lib/src/main/cpp/test_common/Geometry.h b/lib/src/main/cpp/test_common/Geometry.h
index 08947b9..f2cf553 100644
--- a/lib/src/main/cpp/test_common/Geometry.h
+++ b/lib/src/main/cpp/test_common/Geometry.h
@@ -17,37 +17,94 @@
GM_Containment
} relation_t;
-typedef struct PointF_ {
+typedef struct {
double X;
double Y;
} PointF;
-typedef struct Line_ {
+/*typedef struct {
double X1;
double Y1;
double X2;
double Y2;
+} Line;*/
+
+typedef struct {
+ PointF p1;
+ PointF p2;
} Line;
-typedef struct Polygon_ {
- int num;
- PointF *point;
+typedef struct {
+ int num = 0;
+ PointF *point = nullptr;
} Polygon;
-typedef struct Circle_ {
+typedef struct {
PointF centre;
double radius;
} Circle;
-#define MAKE_LINE(a, b, c) { (a).X1=b.X; (a).Y1=b.Y; (a).X2=c.X; (a).Y2=c.Y; }
+//#define MAKE_LINE(a, b, c) { a.X1=b.X; a.Y1=b.Y; a.X2=c.X; a.Y2=c.Y; }
+void MAKE_LINE(Line &a, PointF &b, PointF &c);
- double toRadians(double degree);
- double toDegree(double radians);
- bool isEqual(double a, double b);
- bool isEqual2(double a, double b);
+double toRadians(double degree);
+double toDegree(double radians);
+bool isEqual(double a, double b);
+bool isEqual2(double a, double b);
-void MakeLine(Line *line, const PointF *p1, const PointF *p2);
-void MakePolygon(Polygon *polygon, std::initializer_list<PointF> point_set);
+class MakePolygon {
+public:
+ MakePolygon(std::initializer_list<PointF> points) {
+ polygon.num = 0;
+ if (points.size() > 0) {
+ polygon.point = new PointF[points.size()];
+ int n = 0;
+ for (auto ptr: points) {
+ polygon.point[n++] = ptr;
+ }
+ polygon.num = n;
+ }
+ }
+ MakePolygon(int pre_num) {
+ polygon.num = 0;
+ polygon.point = new PointF[pre_num];
+ }
+
+ ~MakePolygon() {
+ if (polygon.point != nullptr) {
+ delete []polygon.point;
+ }
+ }
+
+ void AddPoints(std::initializer_list<PointF> points) {
+ if (polygon.point != nullptr) {
+ delete []polygon.point;
+ }
+ polygon.num = 0;
+ if (points.size() > 0) {
+ int n = 0;
+ polygon.point = new PointF[points.size()];
+ for (auto p: points) {
+ polygon.point[n++] = p;
+ }
+ polygon.num = n;
+ }
+ }
+
+ void AddPoint(PointF &point) {
+ polygon.point[polygon.num] = point;
+ polygon.num++;
+ }
+
+ Polygon *GetPolygon(void) {
+ return &polygon;
+ }
+
+private:
+ Polygon polygon;
+};
+
+//void MakePolygon(Polygon *polygon, std::initializer_list<PointF> point_set);
void CleanPolygon(Polygon *polygon);
void MakeHidePoint(PointF *point, const PointF *bp, const Line *bl);
@@ -83,7 +140,7 @@
PointF GetVerticalPoint(PointF p1, PointF p2, PointF p3);
bool VerticalPointOnLine(PointF point, Line line);
bool VerticalPointOnLine(PointF point, Line line, PointF &vp);
-PointF Calc3Point(PointF p1, PointF p2, double L, char dir);
+PointF Calc3Point(PointF p1, PointF p2, double length, char dir);
PointF PointExtend(PointF ori, double length, double yaw);
bool IsSamePoint(PointF p1, PointF p2);
double AvgYaw(std::vector<double> &angles);
diff --git a/lib/src/main/cpp/test_common/car_sensor.cpp b/lib/src/main/cpp/test_common/car_sensor.cpp
index 1ee0721..cce53e4 100644
--- a/lib/src/main/cpp/test_common/car_sensor.cpp
+++ b/lib/src/main/cpp/test_common/car_sensor.cpp
@@ -10,6 +10,8 @@
#include "../jni_log.h"
#include "../rtk_platform/platform.h"
+using namespace std;
+
#define DEBUG(fmt, args...) LOGD("<car_sensor> <%s>: " fmt, __func__, ##args)
#define MAX_SENSOR_NUM 32
@@ -45,8 +47,6 @@
SENSOR_SPEED
};
-
-
static int CarStatus[CAR_STATUS_END];
static struct sensor_cfg {
@@ -74,9 +74,9 @@
SensorNum = 0;
memset(SensorConfig, 0, sizeof(SensorConfig));
- memset(CarStatus, 0, sizeof(CarStatus));
- CarStatus[DOOR] = DOOR_CLOSE;
- CarStatus[GEAR] = GEAR_N;
+ for (int i = 0; i < CAR_STATUS_END; ++i) {
+ CarStatus[i] = -1;
+ }
memset(&Sensor, 0, sizeof(Sensor));
@@ -118,36 +118,12 @@
// pthread_mutex_unlock(&sonser_mutex);
}
-void UpdateSensor(uint16_t gpio, uint16_t speed, uint16_t rpm)
-{
- uint16_t chg = 0;
-
- sonser_mutex.lock();
- chg = gpioStore^gpio;
- gpioStore = gpio;
- sonser_mutex.unlock();
-
- WriteCarStatus(OBD_SPEED, speed);
- WriteCarStatus(ENGINE_RPM, rpm);
-
- for (int i = 0; i < SensorNum; ++i) {
- if (chg & BV(SensorConfig[i].gpioId)) {
- int level = 0;
-
- if (gpio & BV(SensorConfig[i].gpioId)) {
- level = 1;
- }
-
- if (level == SensorConfig[i].validLvl) {
- SensorChanged(SensorConfig[i].funId, 1);
- } else {
- SensorChanged(SensorConfig[i].funId, 0);
- }
- }
- }
-}
-
-void UpdateSensor(const car_sensor_t *s)
+/******************************************
+ * 鍒濇寰楀埌鐏厜銆佹尅浣嶇瓑IO淇″彿锛屽悗缁鏍规嵁寤惰繜鍒ゆ柇
+ * 寰楀埌杞悜鐏殑姝ｇ‘鎯呭喌
+ * @param s
+ */
+void UpdateSensorHw(const car_sensor_t *s)
{
WriteCarStatus(OBD_SPEED, s->speed);
WriteCarStatus(ENGINE_RPM, s->engine);
@@ -226,10 +202,11 @@
static void WriteCarStatus(uint16_t id, int value)
{
int old_value;
- lock_guard<std::mutex> lock(status_rw_mutex);
- old_value = CarStatus[id];
- CarStatus[id] = value;
-
+ {
+ lock_guard<std::mutex> lock(status_rw_mutex);
+ old_value = CarStatus[id];
+ CarStatus[id] = value;
+ }
if (id != OBD_SPEED && id != ENGINE_RPM && old_value != value) {
uint8_t buffer[6];
@@ -241,6 +218,10 @@
buffer[5] = BREAK_UINT32(value, 0);
PlatformStatusChanged(SENSOR_CHANGE_EVT, buffer, 6);
+ } else if (id == OBD_SPEED) {
+ UpdateSensor(OBD_SPEED, value);
+ } else if (id == ENGINE_RPM) {
+ UpdateSensor(ENGINE_RPM, value);
}
}
@@ -249,7 +230,7 @@
DEBUG("纭杞悜鐏� 宸� %d 鍙� %d", left_turn_signal, right_turn_signal);
if (!left_turn_signal && !right_turn_signal) {
- WriteCarStatus(TURN_SIGNAL_LAMP, OFF_LIGHT);
+ WriteCarStatus(TURN_SIGNAL_LAMP, LIGHT_OFF);
} else if (left_turn_signal && right_turn_signal) {
WriteCarStatus(TURN_SIGNAL_LAMP, HAZARD_LIGHTS);
} else if (left_turn_signal) {
@@ -261,12 +242,13 @@
static void flashBeamLightClose(apptimer_var_t val)
{
- WriteCarStatus(FLASH_BEAM_LAMP, OFF_LIGHT);
+ WriteCarStatus(FLASH_BEAM_LAMP, LIGHT_OFF);
}
static void SensorChanged(int id, int value)
{
DEBUG("鐘舵�佹敼鍙� %d = %d", id, value);
+
switch (id) {
case SENSOR_LEFT_TURN_SIGNAL: {
left_turn_signal = (bool) (value > 0);
@@ -286,25 +268,25 @@
}
case SENSOR_FOG_LIGHT: {
if (value == 0) {
- WriteCarStatus(FOG_LAMP, OFF_LIGHT);
+ WriteCarStatus(FOG_LAMP, LIGHT_OFF);
} else {
- WriteCarStatus(FOG_LAMP, FOG_LIGHT);
+ WriteCarStatus(FOG_LAMP, LIGHT_ON);
}
break;
}
case SENSOR_CLEARANCE_LIGHT: {
if (value == 0) {
- WriteCarStatus(CLEARANCE_LAMP, OFF_LIGHT);
+ WriteCarStatus(CLEARANCE_LAMP, LIGHT_OFF);
} else {
- WriteCarStatus(CLEARANCE_LAMP, CLEARANCE_LIGHT);
+ WriteCarStatus(CLEARANCE_LAMP, LIGHT_ON);
}
break;
}
case SENSOR_DIPPED_BEAM_LIGHT: {
if (value == 0) {
- WriteCarStatus(DIPPED_BEAM_LAMP, OFF_LIGHT);
+ WriteCarStatus(DIPPED_BEAM_LAMP, LIGHT_OFF);
} else {
- WriteCarStatus(DIPPED_BEAM_LAMP, DIPPED_BEAM_LIGHT);
+ WriteCarStatus(DIPPED_BEAM_LAMP, LIGHT_ON);
}
break;
}
@@ -312,17 +294,17 @@
static uint32_t t1 = 0;
if (value == 0) {
- WriteCarStatus(MAIN_BEAM_LAMP, OFF_LIGHT);
+ WriteCarStatus(MAIN_BEAM_LAMP, LIGHT_OFF);
} else {
- WriteCarStatus(MAIN_BEAM_LAMP, MAIN_BEAM_LIGHT);
+ WriteCarStatus(MAIN_BEAM_LAMP, LIGHT_ON);
}
if (value != 0) {
t1 = AppTimer_GetTickCount();
} else if (AppTimer_GetTickCount() - t1 < D_SEC(3)) {
// 3绉掑唴瀹屾垚浜伃锛岄棯鐏�
- if (ReadCarStatus(FLASH_BEAM_LAMP) != FLASH_BEAM_LIGHT) {
- WriteCarStatus(FLASH_BEAM_LAMP, FLASH_BEAM_LIGHT);
+ if (ReadCarStatus(FLASH_BEAM_LAMP) != LIGHT_ON) {
+ WriteCarStatus(FLASH_BEAM_LAMP, LIGHT_ON);
}
AppTimer_delete(flashBeamLightClose);
AppTimer_add(flashBeamLightClose, D_SEC(2));
@@ -331,41 +313,41 @@
}
case SENSOR_SEATBELT: {
if (value == 0) {
- WriteCarStatus(SEATBELT, EJECT_SEATBELT);
+ WriteCarStatus(SEATBELT, EJECT);
} else {
- WriteCarStatus(SEATBELT, INSERT_SEATBELT);
+ WriteCarStatus(SEATBELT, INSERT);
}
break;
}
case SENSOR_ENGINE_START: {
if (value == 0) {
- WriteCarStatus(ENGINE_START, ENGINE_START_INACTIVE);
+ WriteCarStatus(ENGINE_START, INACTIVE);
} else {
- WriteCarStatus(ENGINE_START, ENGINE_START_ACTIVE);
+ WriteCarStatus(ENGINE_START, ACTIVE);
}
break;
}
case SENSOR_BREAK: {
if (value == 0) {
- WriteCarStatus(BREAK, BREAK_INACTIVE);
+ WriteCarStatus(BRAKE, INACTIVE);
} else {
- WriteCarStatus(BREAK, BREAK_ACTIVE);
+ WriteCarStatus(BRAKE, ACTIVE);
}
break;
}
case SENSOR_BREAK2: {
if (value == 0) {
- WriteCarStatus(SECOND_BREAK, BREAK_INACTIVE);
+ WriteCarStatus(SECOND_BRAKE, INACTIVE);
} else {
- WriteCarStatus(SECOND_BREAK, BREAK_ACTIVE);
+ WriteCarStatus(SECOND_BRAKE, ACTIVE);
}
break;
}
case SENSOR_HANDBREAK: {
if (value == 0) {
- WriteCarStatus(HAND_BREAK, BREAK_INACTIVE);
+ WriteCarStatus(HAND_BRAKE, INACTIVE);
} else {
- WriteCarStatus(HAND_BREAK, BREAK_ACTIVE);
+ WriteCarStatus(HAND_BRAKE, ACTIVE);
}
break;
}
@@ -379,48 +361,36 @@
}
case SENSOR_SURROUND_CAR_1: {
if (value == 0) {
- WriteCarStatus(SURROUND_CAR_1, SURROUND_CAR_INACTIVE);
+ WriteCarStatus(SURROUND_CAR_1, INACTIVE);
} else {
- WriteCarStatus(SURROUND_CAR_1, SURROUND_CAR_ACTIVE);
+ WriteCarStatus(SURROUND_CAR_1, ACTIVE);
}
break;
}
case SENSOR_SURROUND_CAR_2: {
if (value == 0) {
- WriteCarStatus(SURROUND_CAR_2, SURROUND_CAR_INACTIVE);
+ WriteCarStatus(SURROUND_CAR_2, INACTIVE);
} else {
- WriteCarStatus(SURROUND_CAR_2, SURROUND_CAR_ACTIVE);
+ WriteCarStatus(SURROUND_CAR_2, ACTIVE);
}
break;
}
case SENSOR_SURROUND_CAR_3: {
if (value == 0) {
- WriteCarStatus(SURROUND_CAR_3, SURROUND_CAR_INACTIVE);
+ WriteCarStatus(SURROUND_CAR_3, INACTIVE);
} else {
- WriteCarStatus(SURROUND_CAR_3, SURROUND_CAR_ACTIVE);
+ WriteCarStatus(SURROUND_CAR_3, ACTIVE);
}
break;
}
case SENSOR_SURROUND_CAR_4: {
if (value == 0) {
- WriteCarStatus(SURROUND_CAR_4, SURROUND_CAR_INACTIVE);
+ WriteCarStatus(SURROUND_CAR_4, INACTIVE);
} else {
- WriteCarStatus(SURROUND_CAR_4, SURROUND_CAR_ACTIVE);
+ WriteCarStatus(SURROUND_CAR_4, ACTIVE);
}
break;
}
-// case SENSOR_SHIFT_N:
-// case SENSOR_SHIFT_1:
-// case SENSOR_SHIFT_2:
-// case SENSOR_SHIFT_3:
-// case SENSOR_SHIFT_4:
-// case SENSOR_SHIFT_5:
-// case SENSOR_SHIFT_R: {
-// if (value != 0) {
-// WriteCarStatus(GEAR, id - SENSOR_SHIFT_N + GEAR_N);
-// }
-// break;
-// }
default:
return;
}
diff --git a/lib/src/main/cpp/test_common/car_sensor.h b/lib/src/main/cpp/test_common/car_sensor.h
index e15b183..dd6a976 100644
--- a/lib/src/main/cpp/test_common/car_sensor.h
+++ b/lib/src/main/cpp/test_common/car_sensor.h
@@ -20,9 +20,9 @@
MAIN_BEAM_LAMP,
SEATBELT,
ENGINE_START,
- BREAK,
- HAND_BREAK,
- SECOND_BREAK,
+ BRAKE,
+ HAND_BRAKE,
+ SECOND_BRAKE,
DOOR,
SURROUND_CAR_1,
SURROUND_CAR_2,
@@ -31,25 +31,17 @@
CAR_STATUS_END //////////////
};
+#define LIGHT_OFF 0
+#define LIGHT_ON 1
+#define INACTIVE 0
+#define ACTIVE 1
+#define DOOR_OPEN 0
+#define DOOR_CLOSE 1
+#define EJECT 0
+#define INSERT 1
+
// Value
enum {
- OFF_LIGHT = 0,
- EJECT_SEATBELT = 0,
- ENGINE_START_INACTIVE = 0,
- BREAK_INACTIVE = 0,
- DOOR_OPEN = 0,
- SURROUND_CAR_INACTIVE = 0,
-
- HAZARD_LIGHTS,
- LEFT_TURN_LIGHT,
- RIGHT_TURN_LIGHT,
- CLEARANCE_LIGHT,
- DIPPED_BEAM_LIGHT,
- MAIN_BEAM_LIGHT,
- FLASH_BEAM_LIGHT,
- FOG_LIGHT,
- INSERT_SEATBELT,
- ENGINE_START_ACTIVE,
GEAR_N,
GEAR_1,
GEAR_2,
@@ -57,9 +49,9 @@
GEAR_4,
GEAR_5,
GEAR_R,
- BREAK_ACTIVE,
- DOOR_CLOSE,
- SURROUND_CAR_ACTIVE
+ HAZARD_LIGHTS,
+ LEFT_TURN_LIGHT,
+ RIGHT_TURN_LIGHT
};
typedef struct {
@@ -95,8 +87,7 @@
void CarSensorInit(void);
int ReadCarStatus(uint16_t id);
-void UpdateSensor(uint16_t gpio, uint16_t speed, uint16_t rpm);
-void UpdateSensor(const car_sensor_t *s);
+void UpdateSensorHw(const car_sensor_t *s);
void SetSensorCfg(int (*sensor)[3], int sensorNum);
diff --git a/lib/src/main/cpp/test_items/area_exam.cpp b/lib/src/main/cpp/test_items/area_exam.cpp
index bed7a7c..a005b7f 100644
--- a/lib/src/main/cpp/test_items/area_exam.cpp
+++ b/lib/src/main/cpp/test_items/area_exam.cpp
@@ -3,65 +3,158 @@
//
#include <cstdlib>
+#include <tuple>
+#include <map>
+
#include "area_exam.h"
#include "../test_common/car_sensor.h"
#include "../driver_test.h"
#include "../jni_log.h"
#include "park_bottom.h"
-#include "stop_and_start.h"
+#include "uphill.h"
#include "park_edge.h"
#include "driving_curve.h"
-#include "turn_a90.h"
+#include "right_corner.h"
#include "../utils/xconvert.h"
#include "../common/apptimer.h"
#include "../test_common/odo_graph.h"
#include "../test_common/Geometry.h"
#include "../common/observer.h"
+#include "../native-lib.h"
#define DEBUG(fmt, args...) LOGD("<area_exam> <%s>: " fmt, __func__, ##args)
-ilovers::Observer<std::function<void(move_status_t)>> CarMoveEvent;
+enum class ProximityStatus {
+ Near,
+ Bounce,
+ Far
+};
-static bool ProximityArea(Line &base_line, Line &line);
+ilovers::Observer<std::function<void(move_status_t, move_status_t, double)>> CarMoveEvent;
+ilovers::Observer<std::function<void(int, int, double)>> ShiftEvent;
+
+static void CheckProximity(prime_t &prime, Line &base_line, Line &line, int this_id);
static bool CrossingStartLine(Line &trace, Line &start_line);
static void ProximityReminders(prime_t &prime);
static void DetectCarMove(prime_t &prime);
+static void DetectShift(prime_t &prime);
+static void EnterMap(prime_t &prime);
+static void ExitMap(prime_t &prime);
void AreaExam(prime_t &prime)
{
- switch (prime.curr_exam_map.type) {
- case MAP_TYPE_PARK_BUTTOM:
- break;
- case MAP_TYPE_PARK_EDGE:
- break;
- case MAP_TYPE_TURN_90:
- break;
- case MAP_TYPE_UPHILL:
- break;
- case MAP_TYPE_CURVE:
- break;
- default:
- if (EnterMap()) {
-
- }
- break;
+ if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) {
+ return;
}
+ DEBUG("--------------> %d", __LINE__);
+ DetectShift(prime);
+ DEBUG("--------------> %d", __LINE__);
DetectCarMove(prime);
+ DEBUG("--------------> %d", __LINE__);
+ ProximityReminders(prime);
+ DEBUG("--------------> %d", __LINE__);
+ EnterMap(prime);
+ DEBUG("--------------> %d", __LINE__);
+ TestDrivingCurve(prime);
+ DEBUG("--------------> %d", __LINE__);
+ TestParkBottom(prime);
+ DEBUG("--------------> %d", __LINE__);
+ TestParkEdge(prime);
+ DEBUG("--------------> %d", __LINE__);
+ TestUphill(prime);
+ DEBUG("--------------> %d", __LINE__);
+ TestRightCorner(prime);
+ DEBUG("--------------> %d", __LINE__);
+ ExitMap(prime);
+ DEBUG("--------------> %d", __LINE__);
}
+// 妫�鏌ユ崲鎸★紝濡傚仠杞﹀叆搴撱�佸�掕溅寮�濮嬬瓑鍔ㄤ綔锛岄渶瑕佸湪鎸′綅鍙樻崲鏃舵娴嬶紝鍚屾椂闇�鑰冭檻椹鹃┒鍛樹竴娆℃崲鎸′笉杩涳紝鍋滃湪鍘熷湴鍐嶆鎿嶄綔鐨勬儏鍐�
+static void DetectShift(prime_t &prime)
+{
+ static int prevGear = prime.sensor.gear;
+ static double odo = prime.odo; // 璁板綍杩欎釜鎸′綅涓嬭椹剁殑璺濈
+
+ if (prime.sensor.gear != prevGear) {
+ if (prime.odo - odo > 1) {
+
+ }
+ ShiftEvent.Notify(prime.sensor.gear, prevGear, prime.odo - odo);
+ prevGear = prime.sensor.gear;
+ odo = prime.odo;
+ }
+}
+
+static bool stopTimeout = false;
+
+static void StoppedTimeout(apptimer_var_t val) {
+ stopTimeout = true;
+}
+
+// 澶勭悊鍚勪釜椤圭洰鐨勮秴鏃跺仠杞︼紙鍋滀笅鏃跺拰鍐嶆鍚姩鏃讹紝鎸′綅鏄竴鑷寸殑锛�
static void DetectCarMove(prime_t &prime)
{
- static move_status_t prevMove = STOP;
+ static double odo = prime.odo;
+ static int gearWhenStop = GEAR_N;
+ static move_status_t prevMove = prime.pMotion->move;
if (prime.pMotion->move != prevMove) {
// Notify
- CarMoveEvent.Notify(prime.pMotion->move);
+ DEBUG("琛岄┒鐘舵�佹敼鍙� %d", prime.pMotion->move);
+ CarMoveEvent.Notify(prime.pMotion->move, prevMove, prime.odo - odo);
+
+ AppTimer_delete(StoppedTimeout);
+ if (prime.pMotion->move == STOP) {
+ gearWhenStop = prime.sensor.gear;
+ switch (prime.examing_area.type) {
+ case MAP_TYPE_PARK_BUTTOM:
+ AppTimer_add(StoppedTimeout, prime.examParam.park_bottom_pause_criteria);
+ break;
+ case MAP_TYPE_PARK_EDGE:
+ AppTimer_add(StoppedTimeout, prime.examParam.park_edge_pause_criteria);
+ break;
+ case MAP_TYPE_RIGHT_CORNER:
+ AppTimer_add(StoppedTimeout, prime.examParam.turn_a90_pause_criteria);
+ break;
+ case MAP_TYPE_CURVE:
+ AppTimer_add(StoppedTimeout, prime.examParam.curve_pause_criteria);
+ break;
+ case MAP_TYPE_UPHILL:
+ break;
+ default:break;
+ }
+ } else {
+ if (stopTimeout && ((prime.sensor.gear == GEAR_R) == (gearWhenStop == GEAR_R))) {
+ // 鍋滆溅瓒呮椂
+ switch (prime.examing_area.type) {
+ case MAP_TYPE_PARK_BUTTOM:
+ // 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
+ AddExamFault(20106);
+ break;
+ case MAP_TYPE_PARK_EDGE:
+ AddExamFault(20406);
+ break;
+ case MAP_TYPE_RIGHT_CORNER:
+ AddExamFault(20703);
+ break;
+ case MAP_TYPE_CURVE:
+ // 鍋滆溅瓒�2绉掞紝涓嶅悎鏍�
+ AddExamFault(20602);
+ break;
+ case MAP_TYPE_UPHILL:
+ break;
+ default:break;
+ }
+ }
+ }
+ stopTimeout = false;
prevMove = prime.pMotion->move;
+ odo = prime.odo;
}
}
-void RegisterCarMoveObserver(void (*ptr)(move_status_t))
+int RegisterCarMoveObserver(std::function<void(move_status_t, move_status_t, double)> ob)
{
- CarMoveEvent.Connect(ptr);
+ return CarMoveEvent.Connect(ob);
}
void UnregisterCarMoveObserver(int handle)
@@ -69,163 +162,313 @@
CarMoveEvent.Disconnect(handle);
}
-void EnterMap(prime_t &prime)
+int RegisterShiftObserver(std::function<void(int, int, double)> ob)
{
- if (prime.curr_exam_map.type != 0) {
+ return ShiftEvent.Connect(ob);
+}
+
+void UnregisterShiftObserver(int handle)
+{
+ ShiftEvent.Disconnect(handle);
+}
+
+static void EnterMap(prime_t &prime)
+{
+ if (prime.examing_area.type != 0) {
return;
}
- if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) {
- return;
- }
+ DEBUG("--------------> %d", __LINE__);
- PointF &lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
- PointF &lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
- PointF &rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
- PointF &rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
+ DEBUG("1. %d", prime.pModeling[prime.curr_modeling_index].points.size());
+ DEBUG("2. %d", prime.pModeling[prime.prev_modeling_index].points.size());
+
+ DEBUG("3. %d", prime.pModel->left_front_tire[TIRE_OUTSIDE]);
+ DEBUG("4. %d", prime.pModel->right_front_tire[TIRE_OUTSIDE]);
+
+
+ PointF lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
+ PointF lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
+ PointF rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
+ PointF rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
Line left_trace, right_trace;
-
+ DEBUG("--------------> %d", __LINE__);
MAKE_LINE(left_trace, lp1, lp2);
MAKE_LINE(right_trace, rp1, rp2);
Line start_line;
- for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) {
- MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);
+ DEBUG("鍊掑簱> %d", std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).size());
+ for (int i = 0; i < std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).size(); ++i) {
+ MAKE_LINE(start_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[1], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[0]);
if (CrossingStartLine(left_trace, start_line)) {
- prime.curr_exam_map.type = MAP_TYPE_PARK_BUTTOM;
- prime.curr_exam_map.map_idx = i;
+ prime.examing_area.type = MAP_TYPE_PARK_BUTTOM;
+ prime.examing_area.idx = i;
StartParkBottom(prime);
return;
}
- MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[7], prime.pMap->park_button_map[i].map[6]);
+ MAKE_LINE(start_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[7], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[6]);
if (CrossingStartLine(left_trace, start_line)) {
- prime.curr_exam_map.type = MAP_TYPE_PARK_BUTTOM;
- prime.curr_exam_map.map_idx = i;
+ prime.examing_area.type = MAP_TYPE_PARK_BUTTOM;
+ prime.examing_area.idx = i;
StartParkBottom(prime);
return;
}
}
-
- for (int i = 0; i < prime.pMap->park_edge_map.size(); ++i) {
- MAKE_LINE(start_line, prime.pMap->park_edge_map[i].map[1], prime.pMap->park_edge_map[i].map[0]);
+ DEBUG("渚ф柟> %d", std::get<MAP_TYPE_PARK_EDGE>(prime.maps).size());
+ for (int i = 0; i < std::get<MAP_TYPE_PARK_EDGE>(prime.maps).size(); ++i) {
+ MAKE_LINE(start_line, std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[i].points[1], std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[i].points[0]);
if (CrossingStartLine(left_trace, start_line)) {
- prime.curr_exam_map.type = MAP_TYPE_PARK_EDGE;
- prime.curr_exam_map.map_idx = i;
+ prime.examing_area.type = MAP_TYPE_PARK_EDGE;
+ prime.examing_area.idx = i;
+ StartParkEdge(prime);
return;
}
+ }
+ DEBUG("涓婂潯> %d", std::get<MAP_TYPE_UPHILL>(prime.maps).size());
+ for (int i = 0; i < std::get<MAP_TYPE_UPHILL>(prime.maps).size(); ++i) {
+ MAKE_LINE(start_line, std::get<MAP_TYPE_UPHILL>(prime.maps)[i].points[0], std::get<MAP_TYPE_UPHILL>(prime.maps)[i].points[10]);
+ if (CrossingStartLine(left_trace, start_line)) {
+ prime.examing_area.type = MAP_TYPE_UPHILL;
+ prime.examing_area.idx = i;
+ StartUphill(prime);
+ return;
+ }
+ }
+ DEBUG("鏇茬嚎> %d", std::get<MAP_TYPE_CURVE>(prime.maps).size());
+ for (int i = 0; i < std::get<MAP_TYPE_CURVE>(prime.maps).size(); ++i) {
+ MAKE_LINE(start_line, std::get<MAP_TYPE_CURVE>(prime.maps)[i].right_start_point, std::get<MAP_TYPE_CURVE>(prime.maps)[i].left_start_point);
+ if (CrossingStartLine(left_trace, start_line)) {
+ prime.examing_area.type = MAP_TYPE_CURVE;
+ prime.examing_area.idx = i;
+ StartDrivingCurve(prime);
+ return;
+ }
+ }
+ DEBUG("鐩磋> %d", std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps).size());
+ for (int i = 0; i < std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps).size(); ++i) {
+ MAKE_LINE(start_line, std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[i].points[0], std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[i].points[1]);
+ if (CrossingStartLine(left_trace, start_line)) {
+ prime.examing_area.type = MAP_TYPE_RIGHT_CORNER;
+ prime.examing_area.idx = i;
+ StartRightCorner(prime);
+ return;
+ }
+ }
+}
+
+/****************************************
+ * 閫�鍑哄満鍦帮細鏉′欢鏄溅澶村拰鍥涜疆閮戒笉鍦ㄥ満鍦颁腑
+ * 濡傛灉褰撳墠娴嬭瘯椤圭洰鏈畬鎴愶紝瑕佹姤涓嶅悎鏍�
+ * @param prime
+ */
+static void ExitMap(prime_t &prime)
+{
+ if (prime.examing_area.type == MAP_TYPE_NONE) {
+ return;
}
- for (int i = 0; i < prime.pMap->uphill_map.size(); ++i) {
- PointF vPoint = Calc3Point(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[0], DistanceOf(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[7]), 'R');
- MAKE_LINE(start_line, prime.pMap->uphill_map[i].map[0], vPoint);
- if (CrossingStartLine(left_trace, start_line)) {
- prime.curr_exam_map.type = MAP_TYPE_UPHILL;
- prime.curr_exam_map.map_idx = i;
- return;
+ if (prime.examing_area.type == MAP_TYPE_CURVE) {
+ // 杞︾殑鍚庤疆鏄惁瓒婅繃缁撴潫绾挎垨鏄溅杈嗕笉鍦�2涓ぇ鍦嗚寖鍥村唴
+ PointF lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
+ PointF lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
+ PointF rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
+ PointF rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
+
+ Line left_trace, right_trace;
+
+ MAKE_LINE(left_trace, lp1, lp2);
+ MAKE_LINE(right_trace, rp1, rp2);
+
+ Line end_line;
+ MAKE_LINE(end_line, std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].right_end_point, std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].left_end_point);
+ if (CrossingStartLine(left_trace, end_line)) {
+ // 绂诲紑鍖哄煙
+ StopDrivingCurve(prime);
}
- }
- for (int i = 0; i < prime.pMap->curve_map.size(); ++i) {
- MAKE_LINE(start_line, prime.pMap->curve_map[i].right_start_point, prime.pMap->curve_map[i].left_start_point);
- if (CrossingStartLine(left_trace, start_line)) {
- prime.curr_exam_map.type = MAP_TYPE_CURVE;
- prime.curr_exam_map.map_idx = i;
- return;
+
+ if (DistanceOf(prime.pModeling[prime.curr_modeling_index].base_point, std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_big_circle_centre) > std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_big_circle_radius
+ && DistanceOf(prime.pModeling[prime.curr_modeling_index].base_point, std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_big_circle_centre) > std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_big_circle_radius) {
+ // 绂诲紑鍖哄煙
+ StopDrivingCurve(prime);
}
- }
- for (int i = 0; i < prime.pMap->turn_a90_map.size(); ++i) {
- MAKE_LINE(start_line, prime.pMap->turn_a90_map[i].map[0], prime.pMap->turn_a90_map[i].map[1]);
- if (CrossingStartLine(left_trace, start_line)) {
- prime.curr_exam_map.type = MAP_TYPE_TURN_90;
- prime.curr_exam_map.map_idx = i;
- return;
+ } else {
+ MakePolygon area({});
+ DEBUG("------------> %d", __LINE__);
+ switch (prime.examing_area.type) {
+ case MAP_TYPE_PARK_BUTTOM:
+ area.AddPoints({std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[0],
+ std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[1],
+ std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2],
+ std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3],
+ std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[4],
+ std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[5],
+ std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[6],
+ std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[7]});
+ break;
+ case MAP_TYPE_PARK_EDGE:
+ area.AddPoints({std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[0],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[1],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[2],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[3],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[4],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[5],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[6],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[7]});
+ break;
+ case MAP_TYPE_UPHILL:
+ area.AddPoints({std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[0],
+ std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[9],
+ std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[11],
+ std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[10]});
+ break;
+ case MAP_TYPE_RIGHT_CORNER:
+ area.AddPoints({std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[0],
+ std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[1],
+ std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[2],
+ std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[3],
+ std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[4],
+ std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[5]});
+ break;
+ default:
+ break;
}
+
+ DEBUG("------------> %d", __LINE__);
+ int num = 0;
+
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
+ area.GetPolygon()) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
+ area.GetPolygon()) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
+ area.GetPolygon()) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]],
+ area.GetPolygon()) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[prime.pModel->axial[AXIAL_FRONT]]],
+ area.GetPolygon()) == GM_None) {
+ num++;
+ }
+ DEBUG("------------> %d", __LINE__);
+ if (num == 5) {
+ // 绂诲紑鍖哄煙
+ StopDrivingCurve(prime);
+ StopParkBottom(prime);
+ StopRightCorner(prime);
+ StopUphill(prime);
+ StopParkEdge(prime);
+ }
+ DEBUG("------------> %d", __LINE__);
}
}
// 杞﹁疆椹惰繃绾匡紝涓旇溅澶翠綅浜庡彸渚�
static bool CrossingStartLine(Line &trace, Line &start_line)
{
- PointF head = {.X = trace.X1, .Y = trace.Y1};
+ PointF head = trace.p1;
if (IntersectionOf(trace, start_line) == GM_Intersection
- && IntersectionOfLine(head, start_line) == RELATION_RIGHT) {
+ && IntersectionOfLine(head, start_line) == REL_POS_RIGHT) {
return true;
}
return false;
}
-void FarawayMap(prime_t &prime)
-{
- if (prime.arriving_map.type != 0) {
- PointF &car_head = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]];
- PointF car_head_trend = PointExtend(car_head, 7, prime.pModeling->yaw);
- Line car_head_line;
- MAKE_LINE(car_head_line, car_head, car_head_trend);
-
-
- }
-}
-
static void ProximityReminders(prime_t &prime)
{
- if (prime.curr_exam_map.type != MAP_TYPE_NONE && prime.arriving_map.type != MAP_TYPE_NONE) {
+ if (prime.examing_area.type != MAP_TYPE_NONE)
return;
+
+ DEBUG("----------------> %d", __LINE__);
+
+ DEBUG("test %d",std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).size());
+ DEBUG("test %d",std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[0].points.size());
+ DEBUG("test %f,%f",std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[0].points[1].X, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[0].points[1].Y);
+ {
+ Line test;
+
+ MAKE_LINE(test, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[0].points[1], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[0].points[0]);
+
+ DEBUG("test %f,%f - %f,%f",test.p1.X, test.p1.Y, test.p2.X, test.p2.Y);
}
- PointF &car_head = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]];
- PointF car_head_trend = PointExtend(car_head, 6, prime.pModeling->yaw);
+ DEBUG("----------------> %d", __LINE__);
+ PointF car_head = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]];
+ PointF car_head_trend = PointExtend(car_head, 8, prime.pModeling->yaw);
+ DEBUG("----------------> %d", __LINE__);
+
Line car_head_line;
MAKE_LINE(car_head_line, car_head, car_head_trend);
-
+ DEBUG("----------------> %d", __LINE__);
Line start_line;
- for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) { // 宸﹀彸2鏉℃帶鍒剁嚎閮藉彲浣滀负鍏ュ彛
- MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);
-
- MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[7], prime.pMap->park_button_map[i].map[6]);
-
+ for (int i = 0; i < std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).size(); ++i) { // 宸﹀彸2鏉℃帶鍒剁嚎閮藉彲浣滀负鍏ュ彛
+ MAKE_LINE(start_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[1], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[0]);
+ CheckProximity(prime, car_head_line, start_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].id);
+ MAKE_LINE(start_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[7], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[6]);
+ CheckProximity(prime, car_head_line, start_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].id);
}
- for (int i = 0; i < prime.pMap->park_edge_map.size(); ++i) {
- MAKE_LINE(start_line, prime.pMap->park_edge_map[i].map[1], prime.pMap->park_edge_map[i].map[0]);
+ for (int i = 0; i < std::get<MAP_TYPE_PARK_EDGE>(prime.maps).size(); ++i) {
+ MAKE_LINE(start_line, std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[i].points[1], std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[i].points[0]);
+ CheckProximity(prime, car_head_line, start_line, std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[i].id);
}
- for (int i = 0; i < prime.pMap->uphill_map.size(); ++i) {
- PointF vPoint = Calc3Point(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[0], DistanceOf(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[7]), 'R');
- MAKE_LINE(start_line, prime.pMap->uphill_map[i].map[0], vPoint);
+ for (int i = 0; i < std::get<MAP_TYPE_UPHILL>(prime.maps).size(); ++i) {
+ MAKE_LINE(start_line, std::get<MAP_TYPE_UPHILL>(prime.maps)[i].points[0], std::get<MAP_TYPE_UPHILL>(prime.maps)[i].points[10]);
+ CheckProximity(prime, car_head_line, start_line, std::get<MAP_TYPE_UPHILL>(prime.maps)[i].id);
}
- for (int i = 0; i < prime.pMap->curve_map.size(); ++i) {
- MAKE_LINE(start_line, prime.pMap->curve_map[i].right_start_point, prime.pMap->curve_map[i].left_start_point);
+ for (int i = 0; i < std::get<MAP_TYPE_CURVE>(prime.maps).size(); ++i) {
+ MAKE_LINE(start_line, std::get<MAP_TYPE_CURVE>(prime.maps)[i].right_start_point, std::get<MAP_TYPE_CURVE>(prime.maps)[i].left_start_point);
+ CheckProximity(prime, car_head_line, start_line, std::get<MAP_TYPE_CURVE>(prime.maps)[i].id);
}
- for (int i = 0; i < prime.pMap->turn_a90_map.size(); ++i) {
- MAKE_LINE(start_line, prime.pMap->turn_a90_map[i].map[0], prime.pMap->turn_a90_map[i].map[1]);
+ for (int i = 0; i < std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps).size(); ++i) {
+ MAKE_LINE(start_line, std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[i].points[0], std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[i].points[1]);
+ CheckProximity(prime, car_head_line, start_line, std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[i].id);
}
}
// 鍒ゆ柇鏄惁鎺ヨ繎鍦哄湴鐨勮捣濮嬬嚎
// 杞﹀ご瓒嬪娍绾挎槸鍚﹀拰鍏ュ彛绾跨浉浜�
-// 鍜屽叆鍙ｇ嚎鐨勫す瑙�
-static bool ProximityArea(Line &base_line, Line &line)
+static void CheckProximity(prime_t &prime, Line &base_line, Line &line, int this_id)
{
- PointF head = {.X = line.X1, .Y = line.Y1};
+ ProximityStatus status;
- if (IntersectionOf(base_line, line) == GM_Intersection
- && DistanceOf(head, base_line) > 1) {
- double angle = AngleOfTowLine(base_line, line);
- if (angle >= 240 && angle <= 300) {
- return true;
+ if (CrossingStartLine(base_line, line)) {
+ PointF p1 = base_line.p1;
+ PointF p2 = base_line.p2;
+
+ // 澧炲姞璺濈锛岀敤浜庢秷闄ゆ祴閲忚宸甫鏉ョ殑涓嶇ǔ瀹�
+ if (DistanceOf(p1, line) > 0.5 && DistanceOf(p2, line) > 0.5) {
+ status = ProximityStatus::Near;
+ } else {
+ status = ProximityStatus::Bounce;
}
+ } else {
+ status = ProximityStatus::Far;
}
- return false;
+ if (status == ProximityStatus::Near && prime.arriving_map != this_id) {
+ prime.arriving_map = this_id;
+ // 鎶ヨ闊虫彁绀�
+ PlayTTS("鍗冲皢鑰冭瘯", nullptr);
+ } else if (status == ProximityStatus::Far && prime.arriving_map == this_id) {
+ prime.arriving_map = -1;
+ }
}
void TerminateAreaExam(void)
{
- CurrExamMapIndex = -1;
+
}
void InitAreaExam(void)
{
- CurrExamMapIndex = -1;
ResetOdo();
}
diff --git a/lib/src/main/cpp/test_items/area_exam.h b/lib/src/main/cpp/test_items/area_exam.h
index 1259cdf..9134a7c 100644
--- a/lib/src/main/cpp/test_items/area_exam.h
+++ b/lib/src/main/cpp/test_items/area_exam.h
@@ -5,15 +5,16 @@
#ifndef MYAPPLICATION2_AREA_EXAM_H
#define MYAPPLICATION2_AREA_EXAM_H
-#include <vector>
#include "../driver_test.h"
-
+#include <functional>
void InitAreaExam(void);
void TerminateAreaExam(void);
void AreaExam(prime_t &prime);
-void TestAreaGeneral(area_map_t &map, const car_model_t *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, double azimuth, const struct RtkTime *rtkTime);
-void DistanceOfTire2X(std::vector<double> &array, const car_model_t *car, std::vector<Line> line_set);
+int RegisterCarMoveObserver(std::function<void(move_status_t, move_status_t, double)> ob);
+void UnregisterCarMoveObserver(int handle);
+int RegisterShiftObserver(std::function<void(int, int, double)> ob);
+void UnregisterShiftObserver(int handle);
#endif //MYAPPLICATION2_AREA_EXAM_H
diff --git a/lib/src/main/cpp/test_items/driving_curve.cpp b/lib/src/main/cpp/test_items/driving_curve.cpp
index 7ea497a..2da17c5 100644
--- a/lib/src/main/cpp/test_items/driving_curve.cpp
+++ b/lib/src/main/cpp/test_items/driving_curve.cpp
@@ -13,393 +13,82 @@
#include <vector>
#include <cstdlib>
+#include <tuple>
using namespace std;
#define DEBUG(fmt, args...) LOGD("<driving_curve> <%s>: " fmt, __func__, ##args)
-static bool testing = false;
-static int mapIndex = 0;
-static uint32_t stopTimepoint = 0;
-
-static bool reportStopCarTimeout;
-static int prevMoveDirect;
static bool crashRedLine;
-static struct scan_window_t {
- int leftStart;
- int leftEnd;
- int rightStart;
- int rightEnd;
-} scanWindow;
-static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
-static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
-static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model_t *car, struct scan_window_t *zone, int &who);
+static bool CrashRedLine(prime_t &prime);
-void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime)
+void StartDrivingCurve(prime_t &prime)
{
DEBUG("杩涘叆鏇茬嚎琛岄┒鍦哄湴");
- testing = true;
- mapIndex = index;
-
- prevMoveDirect = moveDirect;
- if (moveDirect == 0) {
- stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- }
- reportStopCarTimeout = false;
crashRedLine = false;
- scanWindow.leftStart = scanWindow.leftEnd = scanWindow.rightStart = scanWindow.rightEnd = 0;
-
- MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 1);
+ MA_EnterMap(prime.examing_area.idx, MAP_TYPE_CURVE, 1);
}
-// 鏇茬嚎鍦哄湴鍏抽敭鐐�
-// 鍏ュ彛宸﹀彸涓ょ偣锛屽嚭鍙ｅ乏鍙充袱鐐癸紝鍓嶅崐閮ㄥぇ灏忓渾鍦嗗績鍜岀洿寰勶紝鍚庡崐閮ㄥぇ灏忓渾鍦嗗績鍜岀洿寰勶紝瀹為檯娴嬮噺澶у皬鍦嗗渾蹇冨亸宸湪20cm鍐�
-int TestCurve(const curve_map_t *map, const car_model_t *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
+void TestDrivingCurve(prime_t &prime)
{
- // 鍒ゆ柇鏄惁鍦ㄨ寖鍥村唴锛屽鏋�4涓溅杞兘涓嶅湪鏇茬嚎鍐咃紝瑙嗕负杞﹁締绂诲紑鍦哄湴
- vector<PointF> pxs = {car->carXY[car->left_front_tire[TIRE_OUTSIDE]],
- car->carXY[car->left_rear_tire[TIRE_OUTSIDE]],
- car->carXY[car->right_front_tire[TIRE_OUTSIDE]],
- car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]};
+ if (prime.examing_area.type != MAP_TYPE_CURVE)
+ return;
- int collide = 0;
-
- for (auto px: pxs) {
- if (IntersectionOfLine(map->front_half_small_circle_centre, map->back_half_small_circle_centre, px) == RELATION_RIGHT) {
- // 浣嶄簬鍓嶅崐閮�,瀹為檯鍦哄湴宸﹀彸杈圭嚎璧风偣銆佺粓鐐瑰拰鍦嗗績鍩烘湰涓嶄細鍏辩嚎锛屽彲鑳藉樊寮傝繎涓�绫筹紝鎵�浠ヨ鍒嗗紑璁＄畻
- // 鍒ゆ柇鐨勭偣瓒呰繃宸﹀彸璧风偣锛屽嵆鍦ㄧ嚎鐨勫乏渚�
- if (AngleOf(map->front_half_small_circle_centre, map->left_start_point, px) < 180) {
- if (DistanceOf(map->front_half_small_circle_centre, px) < map->front_half_small_circle_radius) {
- collide = 1;
- continue;
- }
- }
- if (AngleOf(map->front_half_big_circle_centre, map->right_start_point, px) < 180) {
- if (DistanceOf(map->front_half_big_circle_centre, px) > map->front_half_big_circle_radius) {
- collide = 2;
- continue;
- }
- }
- } else {
- // 鍒ゆ柇鐨勭偣涓嶈秴杩囧乏鍙崇粓鐐癸紝鍗冲湪绾跨殑宸︿晶
- if (AngleOf(map->back_half_small_circle_centre, map->right_end_point, px) < 180) {
- if (DistanceOf(map->back_half_small_circle_centre, px) < map->back_half_small_circle_radius) {
- collide = 2;
- continue;
- }
- }
- if (AngleOf(map->back_half_big_circle_centre, map->left_end_point, px) < 180) {
- if (DistanceOf(map->back_half_big_circle_centre, px) > map->back_half_big_circle_radius) {
- collide = 1;
- continue;
- }
- }
- }
- }
-
-}
-
-int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model_t *car, const car_model_t *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
-{
- Polygon tireRect;
- int who = 0;
- vector<double> dtox;
- vector<Line> line_set;
- int s;
-
- MakePolygon(&tireRect, {car->carXY[car->left_front_tire[TIRE_OUTSIDE]],
- car->carXY[car->right_front_tire[TIRE_OUTSIDE]],
- car->carXY[car->right_rear_tire[TIRE_OUTSIDE]],
- car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]});
-
- // 鏇存柊杞﹀ご鎵弿绾�
- if (!UpdateStartLine(&scanWindow, map, map2, &tireRect)) {
- DEBUG("绂诲紑鍦哄湴");
- testing = false;
- goto TEST_END;
- }
-
- // 鏇存柊杞﹀熬鎵弿绾�
- UpdateEndLine(false, &scanWindow, map, map2, &tireRect);
-
- // 璁＄畻杈硅窛
- s = scanWindow.leftStart;
- for (int e = scanWindow.leftStart - 1; e >= scanWindow.leftEnd; --e) {
- Line redLine;
- MakeLine(&redLine, &map->point[s], &map->point[e]);
- line_set.push_back(redLine);
- s = e;
- }
-
- s = scanWindow.rightStart;
- for (int e = scanWindow.rightStart - 1; e >= scanWindow.rightEnd; --e) {
- Line redLine;
- MakeLine(&redLine, &map2->point[s], &map2->point[e]);
- line_set.push_back(redLine);
- s = e;
- }
-
- DistanceOfTire2X(dtox, car, line_set);
-
- MA_SendDistance(dtox[0], dtox[1]);
-
- DEBUG("scanWindow leftStart %d leftEnd %d rightStart %d rightEnd %d", scanWindow.leftStart, scanWindow.leftEnd, scanWindow.rightStart, scanWindow.rightEnd);
-
- if (CrashRedLine(map, map2, car, &scanWindow, who)) {
+ if (CrashRedLine(prime)) {
if (!crashRedLine) {
- crashRedLine = true;
// 杞﹁疆鍘嬭竟绾匡紝涓嶅悎鏍�
- AddExamFault(20601, rtkTime);
DEBUG("杞﹁疆鍘嬭竟绾�");
- if (who == 1) {
- PlayTTS("鍘嬪乏鏇茬嚎", NULL);
- } else if (who == 2) {
- PlayTTS("鍘嬪彸鏇茬嚎", NULL);
- }
+ AddExamFault(20601);
}
+ crashRedLine = true;
} 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 >= CorrectPauseCriteria(examParam.curve_pause_criteria) && !reportStopCarTimeout) {
- // 鍋滆溅瓒�2绉掞紝涓嶅悎鏍�
- AddExamFault(20602, rtkTime);
- DEBUG("涓�斿仠杞�");
- reportStopCarTimeout = true;
- }
- }
-
-TEST_END:
- CleanPolygon(&tireRect);
-
- if (!testing) {
- MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 0);
- }
-
- return testing ? 1 : 0;
}
-static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect)
+void StopDrivingCurve(prime_t &prime)
{
- Line start;
+ if (prime.examing_area.type != MAP_TYPE_CURVE)
+ return;
+ DEBUG("绂诲紑鏇茶椹舵槸鍦哄湴");
- bool update = true;
- int direct = 0;
- int tempLeft = scanWindow.leftStart, tempRight = scanWindow.rightStart;
+ prime.examing_area.type = MAP_TYPE_NONE;
+}
- while (update) {
- update = false;
- MakeLine(&start, &map->point[scanWindow.leftStart], &map2->point[scanWindow.rightStart]);
- if (IntersectionOf(start, tireRect) == GM_None) {
- if (direct != 1) {
- direct = -1;
- // 鍏ュ満鏂瑰悜鎵弿
- tempLeft = scanWindow.leftStart;
- tempRight = scanWindow.rightStart;
+// 鏇茬嚎鍦哄湴鍏抽敭鐐�
+// 鍏ュ彛宸﹀彸涓ょ偣锛屽嚭鍙ｅ乏鍙充袱鐐癸紝鍓嶅崐閮ㄥぇ灏忓渾鍦嗗績鍜岀洿寰勶紝鍚庡崐閮ㄥぇ灏忓渾鍦嗗績鍜岀洿寰勶紝瀹為檯娴嬮噺澶у皬鍦嗗渾蹇冨亸宸湪20cm鍐�
+static bool CrashRedLine(prime_t &prime)
+{
+ // 鍒ゆ柇鏄惁鍦ㄨ寖鍥村唴锛�4涓溅杞彧瑕佹弧瓒冲湪灏忓渾澶栵紝澶у渾鍐咃紝灏辫鏄庢病鏈夊帇绾�
+ PointF tires[] = {prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
+ prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
+ prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
+ prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]};
- if (scanWindow.leftStart > 0) {
- update = true;
- scanWindow.leftStart--;
- }
- if (scanWindow.rightStart > 0) {
- update = true;
- scanWindow.rightStart--;
- }
+ for (int i = 0; i < 4; i++) {
+ if (IntersectionOfLine(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_small_circle_centre,
+ std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_small_circle_centre, tires[i]) == REL_POS_RIGHT) {
+ // 鍓嶅崐閮ㄥ垎
+ if (DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_small_circle_centre, tires[i]) > std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_small_circle_radius
+ && DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_big_circle_centre, tires[i]) < std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_big_circle_radius) {
- if (scanWindow.leftStart <= scanWindow.leftEnd && scanWindow.rightStart <= scanWindow.rightEnd) {
- DEBUG("杞﹁締涓㈠け锛岄噸鏂版悳绱� %d %d", scanWindow.leftStart, scanWindow.rightStart);
-
- // 杞﹁締涓㈠け锛岄噸鏂版悳绱�
- update = false;
- scanWindow.leftEnd = scanWindow.rightEnd = 0;
-
- if (UpdateEndLine(true, &scanWindow, map, map2, tireRect)) {
- DEBUG("鍖归厤鎴愬姛 %d %d", scanWindow.leftStart, scanWindow.leftEnd);
- direct = 0;
- update = true;
- } else {
- DEBUG("鍖归厤澶辫触");
- return false;
- }
- }
- }
- } else {
- if (direct != -1) {
- // 鍑哄満鏂瑰悜鎵弿
- direct = 1;
- if (scanWindow.leftStart < map->num - 1) {
- update = true;
- scanWindow.leftStart++;
- }
- if (scanWindow.rightStart < map2->num - 1) {
- update = true;
- scanWindow.rightStart++;
- }
} else {
- scanWindow.leftStart = tempLeft;
- scanWindow.rightStart = tempRight;
- }
- }
- }
-
- return true;
-}
-
-static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect)
-{
- bool update = true;
- bool crash = false;
-
- int direct = 0;
- int tempLeft = zone->leftEnd;
- int tempRight = zone->rightEnd;
-
- Line end;
-
- while (update) {
- update = false;
- MakeLine(&end, &map->point[zone->leftEnd], &map2->point[zone->rightEnd]);
-
- if (IntersectionOf(end, tireRect) == GM_None) {
- if (direct != -1) {
- // 鍑哄満鏂瑰悜鎵弿
- direct = 1;
- tempLeft = zone->leftEnd;
- tempRight = zone->rightEnd;
-
- if (zone->leftEnd < map->num - 1) {
- update = true;
- zone->leftEnd++;
- }
- if (zone->rightEnd < map2->num - 1) {
- update = true;
- zone->rightEnd++;
- }
+ return true;
}
} else {
- if (!crash) {
- crash = true;
- if (mode) {
- zone->leftStart = zone->leftEnd;
- zone->rightStart = zone->rightEnd;
+ if (DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_small_circle_centre, tires[i]) > std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_small_circle_radius
+ && DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_big_circle_centre, tires[i]) < std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_big_circle_radius) {
- DEBUG("绗竴娆℃帴瑙� %d %d %d", zone->leftStart, zone->leftEnd, tempLeft);
- }
- }
-
- if (direct != 1) {
- // 鍏ュ満鏂瑰悜鎵弿
- direct = -1;
- if (zone->leftEnd > 0) {
- update = true;
- zone->leftEnd--;
- }
- if (zone->rightEnd > 0) {
- update = true;
- zone->rightEnd--;
- }
} else {
- zone->leftEnd = tempLeft;
- zone->rightEnd = tempRight;
+ return true;
}
}
}
- return crash;
-}
-
-bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model_t *car)
-{
-// 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
- bool ret = false;
-
- Polygon carBody;
- Polygon bigMap;
-
- bigMap.num = map->num + map2->num;
- bigMap.point = (PointF *) malloc(bigMap.num * sizeof(PointF));
-
- int i = 0;
- for (; i < map->num; ++i) {
- bigMap.point[i] = map->point[i];
- }
- for (int j = map2->num; j > 0; --j) {
- bigMap.point[i++] = map2->point[j-1];
- }
-
- 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, &bigMap) == GM_None) {
- ret = true;
- }
-
- free(carBody.point);
- free(bigMap.point);
-
- return ret;
-}
-
-// 杞﹁疆鏄惁鍘嬭竟绾�
-static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model_t *car, struct scan_window_t *zone, int &who)
-{
- 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) {
- who = 1;
- return true;
- }
- if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
- who = 1;
- 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) {
- who = 2;
- return true;
- }
- if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
- who = 2;
- return true;
- }
- s = e;
- }
-
- return ret;
+ return false;
}
diff --git a/lib/src/main/cpp/test_items/driving_curve.h b/lib/src/main/cpp/test_items/driving_curve.h
index b841076..d9d4a0d 100644
--- a/lib/src/main/cpp/test_items/driving_curve.h
+++ b/lib/src/main/cpp/test_items/driving_curve.h
@@ -8,11 +8,10 @@
#include "../driver_test.h"
#include <vector>
-using namespace std;
+void StartDrivingCurve(prime_t &prime);
-void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime);
-bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model_t *car);
+void TestDrivingCurve(prime_t &prime);
-int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model_t *car, const car_model_t *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime);
+void StopDrivingCurve(prime_t &prime);
#endif //RTKDRIVERTEST_DRIVING_CURVE_H
diff --git a/lib/src/main/cpp/test_items/park_bottom.cpp b/lib/src/main/cpp/test_items/park_bottom.cpp
index 421ecf5..3424f6e 100644
--- a/lib/src/main/cpp/test_items/park_bottom.cpp
+++ b/lib/src/main/cpp/test_items/park_bottom.cpp
@@ -24,27 +24,15 @@
#include "area_exam.h"
#include "../test_common/car_sensor.h"
#include "../test_common/odo_graph.h"
+#include "../teach/train.h"
+#include "../teach/teach.h"
#include <vector>
#include <cstdlib>
+#include <tuple>
#define DEBUG(fmt, args...) LOGD("<park_bottom> <%s>: " fmt, __func__, ##args)
using namespace std;
-
-enum {
- NONE,
- FIRST_TOUCH_CTRL_LINE,
- FIRST_PARK,
- SECOND_TOUCH_CTRL_LINE,
- SECOND_PARK,
- THIRD_TOUCH_CTRL_LINE
-};
-
-enum {
- TESTING,
- TEST_FAIL, // 鍥犺Е鍙戞煇浜涜鍒欙紝鍦ㄨ溅韬湭瀹屽叏绔嬪満鎯呭喌涓嬶紝鎻愬墠缁堟閮ㄥ垎娴嬭瘯
- TEST_FINISH
-};
typedef enum {
NONE_CTRL_LINE,
@@ -52,151 +40,157 @@
RIGHT_CTRL_LINE
} ctrl_line_t;
-static bool reverseCar = false;
-
-const uint32_t CHECK_PARK_DELAY = 400;
-
-static uint32_t stopTimepoint;
-static int prevMoveDirect;
static bool occurCrashRedLine;
-
-static uint32_t firstReverseTimepoint;
-
static int parkCount;
-static char carray[3];
-static int darray[3];
-static int parkStatus[3];
-
-static int gearAtStop;
-static int currGear;
+static int reverseCnt = 0;
+static bool stopFlag;
static ctrl_line_t prevCrossedCtrlLine;
-static double odo;
+static bool checkCtrlLine, checkParkspace;
+static int handleGearOb, handleMoveOb;
+
+static bool BodyCollidingLine(prime_t &prime);
static ctrl_line_t CrossCtrlLine(prime_t &prime);
-static bool EnterParking(prime_t &prime);
-static bool CrashRedLine(prime_t &prime);
+static bool CheckParkspace(prime_t &prime);
+static void MoveOb(move_status_t curr, move_status_t prev, double distance);
+static void GearOb(int gear, int prevGear, double move_distance);
+enum {
+ PREPARE_REVERSE,
+ REVERSE_LEFT,
+ REVERSE_RIGHT,
+ OBVERSE_LEFT,
+ OBVERSE_RIGHT
+};
+
+/*
+1. 杩涘叆 --- 棣栨鍊掑簱鍓�
+2. 鍊掑簱 ---- 鍋滆溅鍒颁綅鍓嶏紙鍙冲�掑簱锛�
+3. 宸﹀嚭搴�
+4. 鍊掑簱 ---- 鍋滆溅鍒颁綅鍓嶏紙宸﹀�掑簱锛�
+5. 鍙冲嚭搴�(绂诲紑鍦哄湴)
+*/
void StartParkBottom(prime_t &prime)
{
- DEBUG("StartParkBottom");
-
- reverseCar = false;
- memset(carray, 0, sizeof(carray));
- memset(darray, 0, sizeof(darray));
- memset(parkStatus, 0, sizeof(parkStatus));
- prevMoveDirect = prime.pMotion->move;
-
- firstReverseTimepoint = 0;
-
+ DEBUG("杩涘叆鍊掑簱鍦哄湴");
+ stopFlag = false;
parkCount = 0;
-
+ reverseCnt = 0;
+ checkCtrlLine = false;
+ checkParkspace = false;
occurCrashRedLine = false;
-
- currGear = ReadCarStatus(GEAR);
+ prime.examing_area.stage = PREPARE_REVERSE;
+ testAll();
+ LoadStageTips(prime);
prevCrossedCtrlLine = NONE_CTRL_LINE;
- stopTimepoint = 0;
-
- odo = ReadOdo();
-
+ handleGearOb = RegisterShiftObserver(GearOb);
+ handleMoveOb = RegisterCarMoveObserver(MoveOb);
PlayTTS("鎮ㄥ凡杩涘叆鍊掕溅鍏ュ簱鍖哄煙", NULL);
+}
+
+static void ParkTimeout(apptimer_var_t var)
+{
+ DEBUG("椤圭洰瓒呮椂");
+ AddExamFault(20105);
+}
+
+static void MoveOb(move_status_t curr, move_status_t prev, double distance)
+{
+ // 杞﹁締鐢辫繍鍔ㄥ埌鍋滄锛岃褰曚竴涓嬶紝褰撴崲鎸″彂鐢熸椂锛岄伩鍏嶅師鍦版潵鍥炴崲鎸￠�犳垚璇垽
+ if (curr == STOP && distance > 0.5) {
+ stopFlag = true;
+ }
+}
+
+static void GearOb(int gear, int prevGear, double move_distance)
+{
+ DEBUG("GearOb %d %f", gear, move_distance);
+
+ if (gear == GEAR_R && stopFlag) { // 绉诲叆鍊掓尅
+ stopFlag = false;
+ reverseCnt++;
+ if (reverseCnt == 1) {
+ AppTimer_delete(ParkTimeout);
+ AppTimer_add(ParkTimeout, GetPrime().examParam.park_bottom_limit_time);
+ }
+ checkCtrlLine = true;
+ } else if (prevGear == GEAR_R && stopFlag) { // 浠庡�掓尅绉诲嚭
+ stopFlag = false;
+ checkParkspace = true;
+ }
+}
+
+void StopParkBottom(prime_t &prime)
+{
+ if (prime.examing_area.type != MAP_TYPE_PARK_BUTTOM)
+ return;
+
+ DEBUG("绂诲紑鍊掑簱鍦哄湴");
+
+ if (parkCount < 2) {
+ DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
+ AddExamFault(10103);
+ }
+ AppTimer_delete(ParkTimeout);
+ prime.examing_area.type = MAP_TYPE_NONE;
+ UnregisterShiftObserver(handleGearOb);
+ UnregisterCarMoveObserver(handleMoveOb);
}
void TestParkBottom(prime_t &prime)
{
- ctrl_line_t crossCtrlLine = NONE_CTRL_LINE;
- uint32_t tp = AppTimer_GetTickCount();
+ if (prime.examing_area.type != MAP_TYPE_PARK_BUTTOM)
+ return;
- vector<double> dtox;
- vector<Line> line_set;
- Line distance_line;
+ train(prime);
- bool gear_change = false;
- int gear = ReadCarStatus(GEAR);
+ if (checkCtrlLine) {
+ ctrl_line_t crossCtrlLine = CrossCtrlLine(prime);
- if (gear == GEAR_R) {
- if (currGear != GEAR_R) {
- gear_change = true;
- currGear = GEAR_R;
- }
- } else if (currGear == GEAR_R) {
- gear_change = true;
- currGear = gear;
- }
-
- if (gear_change) {
- // 妫�鏌ヤ笂涓�娆℃尅浣嶇殑琛岄┒璺濈锛岃繃灏忓氨鏀惧純锛岄伩寮�瀛﹀憳鍘熷湴鎸傛尅閲嶈瘯
- double run_distance = ReadOdo() - odo;
-
- DEBUG("2娆℃尅浣嶈繍琛岃窛绂� %f", run_distance);
-
- if (run_distance < 1) {
- gear_change = false;
- DEBUG("2娆℃尅浣嶈繍琛岃窛绂昏繃灏忥紝蹇界暐");
- }
- odo = ReadOdo();
- }
-
- if (gear_change) {
- if (currGear == GEAR_R) {
- // 鎸傚�掓尅,妫�娴嬫槸鍚﹁繃鎺у埗绾�
- DEBUG("寮�濮嬫寕鍊掓尅");
- if (!reverseCar) {
- DEBUG("寮�濮嬮杞叆搴�");
- reverseCar = true;
- MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 1);
- firstReverseTimepoint = tp; // 寮�濮�210绉掕鏃�
- }
- crossCtrlLine = CrossCtrlLine(prime);
-
- if (parkCount >= 2) {
- DEBUG("寮�濮嬫杞叆搴�");
- parkCount = 0;
- MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 1);
- firstReverseTimepoint = tp; // 寮�濮�210绉掕鏃�
- }
-
- if (crossCtrlLine == NONE_CTRL_LINE) {
- // 鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎
- AddExamFault(20104);
- DEBUG("鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎");
- } else if (crossCtrlLine == prevCrossedCtrlLine) {
- // 閲嶅璺ㄨ秺鍚屼竴鎺у埗绾匡紝涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮忥紝涓嶅悎鏍�
- AddExamFault(20101);
- DEBUG("涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮忥紝 鍚� %c 渚�", prevCrossedCtrlLine);
- } else {
- prevCrossedCtrlLine = crossCtrlLine;
- DEBUG("寮�濮� %c 渚� 鍊掑簱", prevCrossedCtrlLine);
- }
+ if (crossCtrlLine == NONE_CTRL_LINE) {
+ // 鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎
+ DEBUG("鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎");
+ AddExamFault(20104);
+ } else if (crossCtrlLine == prevCrossedCtrlLine) {
+ // 閲嶅璺ㄨ秺鍚屼竴鎺у埗绾匡紝涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮忥紝涓嶅悎鏍�
+ AddExamFault(20101);
+ DEBUG("涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮忥紝 鍚� %d 渚�", prevCrossedCtrlLine);
} else {
- // 浠庡�掓尅绉诲嚭锛屾娴嬫槸鍚﹀叆搴�
- DEBUG("浠庡�掓尅绉诲嚭");
-
- parkCount++;
-
- DEBUG("搴撲綅妫�鏌ユ鏁� = %d", parkCount);
-
- if (EnterParking(prime)) {
- DEBUG("鍊掑簱鎴愬姛");
- } else {
- AddExamFault(20103);
- DEBUG("鍊掑簱涓嶅叆");
- }
+ prevCrossedCtrlLine = crossCtrlLine;
+ DEBUG("寮�濮� %d 渚� 鍊掑簱", prevCrossedCtrlLine);
}
+
+ if (DeltaYaw(YawOf(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3],
+ std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[4]), prime.pModeling[prime.curr_modeling_index].yaw) < 90) {
+ DEBUG("鍙充晶鍊掑簱");
+ prime.examing_area.stage = REVERSE_RIGHT;
+ } else {
+ DEBUG("宸︿晶鍊掑簱");
+ prime.examing_area.stage = REVERSE_LEFT;
+ }
+ LoadStageTips(prime);
+ checkCtrlLine = false;
}
- if (ExitParkArea(prime)) {
- // 绂诲紑鍦哄湴
- DEBUG("绂诲紑鍦哄湴");
- if (parkCount < 2) {
- AddExamFault(10103);
- DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
+ if (checkParkspace) {
+ if (CheckParkspace(prime)) {
+ DEBUG("鍊掑簱鎴愬姛");
+ } else {
+ DEBUG("鍊掑簱涓嶅叆");
+ AddExamFault(20103);
}
- prime.curr_exam_map.type = 0;
- goto TEST_END;
+ parkCount++;
+ if (prime.examing_area.stage == REVERSE_RIGHT) {
+ prime.examing_area.stage = OBVERSE_LEFT; // 浠庡乏渚у�掑簱
+ } else {
+ prime.examing_area.stage = OBVERSE_RIGHT; // 绂诲紑鍦哄湴
+ }
+ LoadStageTips(prime);
+ checkParkspace = false;
}
- if (BodyCollidingLine(prime) >= 0) {
+ if (BodyCollidingLine(prime)) {
if (!occurCrashRedLine) {
occurCrashRedLine = true;
// 杞﹁韩鍑虹嚎锛屼笉鍚堟牸
@@ -207,76 +201,35 @@
occurCrashRedLine = false;
}
- if (prime.pMotion->move != prevMoveDirect) {
- if (prime.pMotion->move == STOP) {
- stopTimepoint = tp;
- gearAtStop = (currGear == GEAR_R ? 1 : 0);
- DEBUG("鍋滆溅浜�");
- DEBUG("鍋滆溅鏃舵尅浣� = %d", gearAtStop);
- } else if (prevMoveDirect == STOP && stopTimepoint > 0) {
- DEBUG("缁х画琛岄┒");
-
- DEBUG("鍋滆溅鏃堕棿 %ld", tp - stopTimepoint);
- DEBUG("鍐嶆绉诲姩鏃舵尅浣� = %d", currGear == GEAR_R ? 1 : 0);
-
- if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_bottom_pause_criteria)
- && gearAtStop == (currGear == GEAR_R ? 1 : 0)) {
- // 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
- AddExamFault(20106);
- DEBUG("涓�斿仠杞�");
- }
- }
-
- prevMoveDirect = prime.pMotion->move;
- }
-TEST_END:
- DEBUG("鍊掑簱缁撴潫");
- MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 0);
+ teach(prime);
}
-static int BodyCollidingLine(prime_t &prime)
+// 杞﹁韩鎴栬溅杞殕涓嶅緱纰拌Е瀹炵嚎
+static bool BodyCollidingLine(prime_t &prime)
{
- vector<Line> lines;
+ MakePolygon car_body(prime.pModel->body.size());
- Polygon car_body;
-
- car_body.num = prime.pModel->body.size();
- car_body.point = new PointF[car_body.num];
-
- for (int i = 0; i < car_body.num; ++i) {
- car_body.point[i] = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]];
+ for (int i = 0; i < prime.pModel->body.size(); ++i) {
+ car_body.AddPoint(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]]);
}
- Line line;
- MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[0],
- prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[7]);
- lines.push_back(line);
- MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[1],
- prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2]);
- lines.push_back(line);
- MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2],
- prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3]);
- lines.push_back(line);
- MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3],
- prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4]);
- lines.push_back(line);
- MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4],
- prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5]);
- lines.push_back(line);
- MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5],
- prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[6]);
- lines.push_back(line);
+ Line frontAxle, rearAxle;
- int idx = 0;
+ MAKE_LINE(frontAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]);
+ MAKE_LINE(rearAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);
- for (auto line: lines) {
- if (IntersectionOf(line, &car_body) != GM_None) {
- break;
+ const int red_lines[][2] = {{0, 7}, {1, 2}, {2, 3}, {3, 4}, {4, 5}, {5, 6}};
+
+ for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
+ Line red_line;
+ MAKE_LINE(red_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[red_lines[i][0]], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[red_lines[i][1]]);
+ if (IntersectionOf(red_line, frontAxle) == GM_Intersection
+ || IntersectionOf(red_line, rearAxle) == GM_Intersection
+ || IntersectionOf(red_line, car_body.GetPolygon()) != GM_None) {
+ return true;
}
- idx++;
}
- delete []car_body.point;
- return idx < lines.size()? idx : -1;
+ return false;
}
// 妫�娴�2鍓嶈疆鏄惁姝ｅ悜瓒婅繃宸﹀彸鎺у埗绾�
@@ -291,95 +244,37 @@
MAKE_LINE(right_trace, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);
- MAKE_LINE(left_ctrl, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[1], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[0]);
- MAKE_LINE(right_ctrl, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[6], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[7]);
+ MAKE_LINE(left_ctrl, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[1], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[0]);
+ MAKE_LINE(right_ctrl, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[6], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[7]);
if (IntersectionOf(left_trace, left_ctrl) == GM_Intersection &&
IntersectionOf(right_trace, left_ctrl) == GM_Intersection &&
- IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], left_ctrl) == RELATION_LEFT) {
+ IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], left_ctrl) == REL_POS_LEFT) {
return LEFT_CTRL_LINE;
}
if (IntersectionOf(left_trace, right_ctrl) == GM_Intersection &&
IntersectionOf(right_trace, right_ctrl) == GM_Intersection &&
- IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], right_ctrl) == RELATION_RIGHT) {
+ IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], right_ctrl) == REL_POS_RIGHT) {
return RIGHT_CTRL_LINE;
}
return NONE_CTRL_LINE;
}
-// 闇�瑕佸簱鍏ュ彛绾垮璁＄畻鍦ㄥ唴
-static bool EnterParking(prime_t &prime) {
- bool succ = false;
+static bool CheckParkspace(prime_t &prime) {
+ MakePolygon car_body(prime.pModel->body.size());
- Polygon park_area;
- Polygon car_body;
-
- car_body.num = prime.pModel->body.size();
- car_body.point = new PointF[car_body.num];
- for (int i = 0; i < car_body.num; ++i) {
- car_body.point[i] = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]];
+ for (int i = 0; i < prime.pModel->body.size(); ++i) {
+ car_body.AddPoint(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]]);
}
- PointF p8 = PointExtend(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2],
- prime.pMap->park_button_map[prime.curr_exam_map.map_idx].line_width,
- YawOf(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3]));
+ MakePolygon park_area({std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[8],
+ std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3],
+ std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[4],
+ std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[9]});
- PointF p9 = PointExtend(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5],
- prime.pMap->park_button_map[prime.curr_exam_map.map_idx].line_width,
- YawOf(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4]));
-
- MakePolygon(&park_area, {p8, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4], p9});
-
- if (IntersectionOf(&car_body, &park_area) == GM_Containment) {
- succ = true;
- }
-
- CleanPolygon(&park_area);
- free(car_body.point);
-
- DEBUG("妫�鏌ュ�掑簱鐘舵�� %s", succ ? "鎴愬姛" : "澶辫触");
-
- return succ;
+ return (bool) (IntersectionOf(car_body.GetPolygon(), park_area.GetPolygon()) == GM_Containment);
}
-// 4涓溅杞拰杞﹀ご鐐逛笉鍦ㄥ満鍦颁腑
-bool ExitParkArea(prime_t &prime)
-{
- Polygon polygon;
-
- polygon.num = prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map.size();
- polygon.point = new PointF[polygon.num];
- for (int i = 0; i < polygon.num; ++i) {
- polygon.point[i] = prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[i];
- }
-
- int num = 0;
-
- if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
- &polygon) == GM_None) {
- num++;
- }
- if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
- &polygon) == GM_None) {
- num++;
- }
- if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
- &polygon) == GM_None) {
- num++;
- }
- if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]],
- &polygon) == GM_None) {
- num++;
- }
- if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[prime.pModel->axial[AXIAL_FRONT]]],
- &polygon) == GM_None) {
- num++;
- }
-
- delete []polygon.point;
-
- return num == 5? true : false;
-}
diff --git a/lib/src/main/cpp/test_items/park_bottom.h b/lib/src/main/cpp/test_items/park_bottom.h
index 35eb634..78c9cea 100644
--- a/lib/src/main/cpp/test_items/park_bottom.h
+++ b/lib/src/main/cpp/test_items/park_bottom.h
@@ -7,12 +7,10 @@
#include "../test_common/Geometry.h"
#include "../driver_test.h"
-#include <vector>
-using namespace std;
void StartParkBottom(prime_t &prime);
void TestParkBottom(prime_t &prime);
-bool ExitParkArea(prime_t &prime);
+void StopParkBottom(prime_t &prime);
#endif //RTKDRIVERTEST_PARK_BOTTOM_H
diff --git a/lib/src/main/cpp/test_items/park_edge.cpp b/lib/src/main/cpp/test_items/park_edge.cpp
index 9ac9060..9c2a5c6 100644
--- a/lib/src/main/cpp/test_items/park_edge.cpp
+++ b/lib/src/main/cpp/test_items/park_edge.cpp
@@ -6,7 +6,7 @@
// | |
// | |
// | |
-// ________________| |___________1
+// ________________|9 8|___________1
// 6 5 2
//
//
@@ -27,425 +27,176 @@
#include <vector>
#include <cstdlib>
+#include <tuple>
#define DEBUG(fmt, args...) LOGD("<park_edge> <%s>: " fmt, __func__, ##args)
using namespace std;
-enum {
- TESTING,
- TEST_FAIL, // 鍥犺Е鍙戞煇浜涜鍒欙紝鍦ㄨ溅韬湭瀹屽叏绔嬪満鎯呭喌涓嬶紝鎻愬墠缁堟閮ㄥ垎娴嬭瘯
- TEST_FINISH
-};
+static bool overpass_parkingspace;
+static bool check_parking_space;
const uint32_t CHECK_PARK_DELAY = 400;
-static bool reportExamTimeout;
-static bool reportParkFail;
+static bool occurCrashRedLine;
+static bool firstMoveBack, checkLight;
-static uint32_t stopTimepoint = 0;
-static bool occurCrashRedLine1, occurCrashRedLine2, occurCrashRedLine3;
-static int prevMoveStatus, storeMoveStatusBeforeStop;
-static int parkStatus;
-static int gearAtStop;
-static bool occurMoveBack, checkPark, parkSuccess, checkLight;
-static uint32_t moveBackTimePoint;
-static int testStatus;
-static int exitAreaCfm;
-static int currGear;
-static double odo;
+static void ParkTimeout(apptimer_var_t val);
+static bool CrashRedLine(prime_t &prime);
+static bool CheckParkspace(prime_t &prime);
+static bool OverpassParkspace(prime_t &prime);
+static void MotionChange(move_status_t mv);
-static bool CrashRedLine1(const Polygon *map, const car_model_t *car);
-static bool CrashRedLine2(const Polygon *map, const car_model_t *car);
-static bool CrashRedLine3(const Polygon *map, const car_model_t *car);
-static bool EnterParking(const Polygon *map, const car_model_t *car);
-static bool ExitParkArea(const Polygon *map, const car_model_t *car);
-static bool ExitParkArea2(const Polygon *map, const car_model_t *car);
+enum {
+ PE_PREPARE_PARK,
+ PE_PARKING,
+ PE_EXIT
+};
void StartParkEdge(prime_t &prime)
{
DEBUG("杩涘叆渚ф柟鍋滆溅鍦哄湴");
- testStatus = TESTING;
-
- occurCrashRedLine1 = occurCrashRedLine2 = occurCrashRedLine3 = false; // 杩欎釜绉戠洰瑙勫畾鐗规畩鐐癸紝鍙戠敓涓�娆℃墸10鍒嗭紝鑰屼笉鐩存帴娣樻卑
- reportExamTimeout = false;
- reportParkFail = false;
- prevMoveStatus = prime.pMotion->move;
- parkSuccess = false;
- parkStatus = 0;
- occurMoveBack = false;
- checkPark = false;
+ occurCrashRedLine = false; // 杩欎釜绉戠洰瑙勫畾鐗规畩鐐癸紝鍙戠敓涓�娆℃墸10鍒嗭紝鑰屼笉鐩存帴娣樻卑
+ firstMoveBack = false;
checkLight = false;
- gearAtStop = -1;
- stopTimepoint = 0;
- odo = ReadOdo();
- currGear = ReadCarStatus(GEAR);
- exitAreaCfm = 0;
-
+ check_parking_space = false;
+ prime.examing_area.stage = PE_PREPARE_PARK;
PlayTTS("鎮ㄥ凡杩涘叆渚ф柟鍋滆溅鍖哄煙", NULL);
- // 浠呭綋鍙戠敓鍊掕溅锛屾墠鎰忓懗鐫�椤圭洰寮�濮�
- /*if (moveStatus == -1) {
- occurMoveBack = true;
- moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- }*/
}
-int TestParkEdge(prime_t &prime)
+void StopParkEdge(prime_t &prime)
{
- vector<double> dtox;
- vector<Line> line_set;
- Line distance_line;
+ if (prime.examing_area.type != MAP_TYPE_PARK_EDGE)
+ return;
+ DEBUG("绂诲紑渚ф柟鍋滆溅鍦哄湴");
- bool gear_change = false;
- int gear = ReadCarStatus(GEAR);
-
- if (gear == GEAR_R) {
- if (currGear != GEAR_R) {
- gear_change = true;
- currGear = GEAR_R;
- }
- } else if (currGear == GEAR_R) {
- gear_change = true;
- currGear = gear;
+ if (!check_parking_space) {
+ DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
+ AddExamFault(10103);
}
+ prime.examing_area.type = MAP_TYPE_NONE;
- if (gear_change) {
- // 妫�鏌ヤ笂涓�娆℃尅浣嶇殑琛岄┒璺濈锛岃繃灏忓氨鏀惧純锛岄伩寮�瀛﹀憳鍘熷湴鎸傛尅閲嶈瘯
- double run_distance = ReadOdo() - odo;
-
- DEBUG("2娆℃尅浣嶈繍琛岃窛绂� %f", run_distance);
-
- if (run_distance < 1) {
- gear_change = false;
- DEBUG("2娆℃尅浣嶈繍琛岃窛绂昏繃灏忥紝蹇界暐");
- }
- odo = ReadOdo();
- }
-
- // 棣栨鎸傚�掓尅锛� 鎵嶆剰鍛崇潃椤圭洰寮�濮�
- if (testStatus == TESTING && gear_change) {
- if (currGear == GEAR_R) {
- if (!occurMoveBack) {
- DEBUG("棣栨渚ф柟鍋滆溅");
- } else {
- DEBUG("鍐嶆渚ф柟鍋滆溅");
- }
-
- {
- checkPark = false;
- occurMoveBack = true;
- moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss,
- rtkTime->mss * 10); // 寮�濮嬭鏃�
- MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 1);
- }
- } else {
- if (occurMoveBack && !checkPark) {
- // 妫�鏌ヨ溅韬叆搴撴儏鍐�
- DEBUG("妫�鏌ヨ溅韬叆搴撴儏鍐�");
- checkPark = true;
- checkLight = false;
- if (EnterParking(map, car)) {
- parkStatus = 1;
- parkSuccess = true;
- } else {
- // 鍋滄鍚庯紝杞﹁韩鍑虹嚎锛屼笉鍚堟牸
- AddExamFault(20401, rtkTime);
- parkSuccess = false;
- DEBUG("绉诲簱涓嶅叆");
- }
- }
- }
- }
-
- if (CrashRedLine1(map, car)) {
- if (!occurCrashRedLine1 /*&& occurMoveBack*/) {
- // 杞﹁疆鍘嬭竟绾匡紝姣忔鎵�10鍒�
- AddExamFault(20403, rtkTime);
- DEBUG("杞﹁疆鍘嬭竟绾�");
- occurCrashRedLine1 = true;
- }
- } else {
- occurCrashRedLine1 = false;
- }
-
- if (CrashRedLine2(map, car)) {
- if (!occurCrashRedLine2 /*&& occurMoveBack*/) {
- // 杞﹁韩鍘嬪簱浣嶇嚎锛屾瘡娆℃墸10鍒�
- AddExamFault(20404, rtkTime);
- DEBUG("杞﹁韩鍘嬪簱浣嶇嚎");
- occurCrashRedLine2 = true;
- }
- } else {
- occurCrashRedLine2 = false;
- }
-
- if (CrashRedLine3(map, car)) {
- if (!occurCrashRedLine3 && !occurMoveBack && moveStatus == 1) {
- // 杞﹁韩鍘嬪簱浣嶇嚎锛屾瘡娆℃墸10鍒�
- AddExamFault(20407, rtkTime);
- DEBUG("杞﹁韩鍘嬪簱浣嶇嚎");
- occurCrashRedLine3 = true;
- }
- } else {
- occurCrashRedLine3 = false;
- }
-
- if (ExitParkArea2(map, car)) {
- /*if (!parkSuccess && occurMoveBack && !reportParkFail) {
- // 鐩存帴椹剁娴嬭瘯鍖猴紝璁や负绉诲簱涓嶅叆
- AddExamFault(10103, rtkTime);
- reportParkFail = true;
- DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
- }*/
- if (occurMoveBack && !checkPark) {
- // 鍊掕溅鐩存帴椹剁娴嬭瘯鍖�
- AddExamFault(10103, rtkTime);
- DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
- }
-
- testStatus = TEST_FINISH;
- goto TEST_END;
- }
-
- // 璺濈妫�娴�
- MakeLine(&distance_line, &map->point[0], &map->point[7]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[1], &map->point[2]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[2], &map->point[3]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[3], &map->point[4]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[4], &map->point[5]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[5], &map->point[6]);
- line_set.push_back(distance_line);
- DistanceOfTire2X(dtox, car, line_set);
- MA_SendDistance(dtox[0], dtox[1]);
-
- if (testStatus == TESTING && !occurMoveBack && ExitParkArea(map, car)) {
- // 鍏ュ簱鍚庝竴鐩村墠杩涳紝杞﹀ご绉诲嚭椹剁绾�
- if (++exitAreaCfm >= 4) { // 閬垮厤淇″彿婕傜Щ閫犳垚鐨勮鍒�
- AddExamFault(10103, rtkTime);
- DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
- testStatus = TEST_FAIL;
- }
- } else {
- exitAreaCfm = 0;
- }
-
- if (occurMoveBack) {
- uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
-
- if (!reportExamTimeout && tp - moveBackTimePoint >= examParam.park_edge_limit_time) {
- // 瓒呮椂90绉掞紝涓嶅悎鏍�
- AddExamFault(20402, rtkTime);
- reportExamTimeout = true;
- DEBUG("绉诲簱90绉掕秴鏃�");
- }
- }
-
- if (moveStatus != prevMoveStatus) {
- if (moveStatus == 0) {
- parkStatus = 0;
- stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- storeMoveStatusBeforeStop = prevMoveStatus;
- gearAtStop = (currGear == GEAR_R ? 1 : 0);
- DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
- DEBUG("鍋滆溅鏃舵尅浣� = %d", gearAtStop);
- } else if (prevMoveStatus == 0 && stopTimepoint > 0) {
- DEBUG("缁х画琛岄┒ %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
- uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
-
- DEBUG("鍋滆溅鏃堕棿 %ld", tp - stopTimepoint);
- DEBUG("鍐嶆绉诲姩鏃舵尅浣� = %d", currGear == GEAR_R ? 1 : 0);
-
- if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_edge_pause_criteria)
- && occurMoveBack
- && gearAtStop == (currGear == GEAR_R ? 1 : 0)) {
- // 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
- AddExamFault(20406, rtkTime);
- DEBUG("鍋滆溅瓒呮椂");
- }
-
- if (moveStatus == 1 && checkPark && !checkLight) {
- // 鍦ㄨ繖閲屾鏌ヨ浆鍚戠伅鐘舵��
- checkLight = true;
- if (ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT) {
- // 涓嶅紑杞悜鐏紝鎵�10鍒�
- AddExamFault(20405, rtkTime);
- DEBUG("鏈紑鍚浆鍚戠伅");
- }
- }
-
- /*if (moveStatus == storeMoveStatusBeforeStop) {
- // 鍚屾柟鍚戝啀鍚姩锛岀户缁垽鏂槸鍚﹀仠杞﹁秴鏃�
- if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_edge_pause_criteria) && occurMoveBack) {
- // 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
- AddExamFault(20406, rtkTime);
- DEBUG("鍋滆溅瓒呮椂");
- }
- } else {
- // 鍊掕溅鍒囨崲鍒板墠杩�
- if (moveStatus == 1 && tp - stopTimepoint >= CHECK_PARK_DELAY) {
- if (parkStatus == 0) {
- if (EnterParking(map, car)) {
- parkStatus = 1;
- parkSuccess = true;
- }
- }
-
- if (!parkSuccess && !reportParkFail) {
- // 鍋滄鍚庯紝杞﹁韩鍑虹嚎锛屼笉鍚堟牸
- AddExamFault(20401, rtkTime);
- reportParkFail = true;
- DEBUG("绉诲簱涓嶅叆");
- }
-
- // 鍦ㄨ繖閲屾鏌ヨ浆鍚戠伅鐘舵��
- if (ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT) {
- // 涓嶅紑杞悜鐏紝鎵�10鍒�
- AddExamFault(20405, rtkTime);
- DEBUG("鏈紑鍚浆鍚戠伅");
- }
- }
- }*/
-
- /* if (moveStatus == -1 && !occurMoveBack) {
- DEBUG("寮�濮嬪�掕溅");
- occurMoveBack = true;
- moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 1);
- }*/
- }
-
- prevMoveStatus = moveStatus;
- } /*else if (moveStatus == 0 && parkStatus == 0) {
- uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
-
- if (tp - stopTimepoint >= CHECK_PARK_DELAY) {
- if (EnterParking(map, car)) {
- parkStatus = 1;
- parkSuccess = true;
- } else {
- parkStatus = -1;
- }
- }
- }*/
-
-TEST_END:
- if (testStatus == TEST_FINISH) {
- DEBUG("渚ф柟鍋滆溅缁撴潫");
- MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 0);
- return 0;
- }
- return 1;
+ AppTimer_delete(ParkTimeout);
}
-int EnterParkEdge(prime_t &prime) {
- if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) {
- return -1;
- }
-
- PointF &lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
- PointF &lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
- PointF &rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
- PointF &rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
-
- Line left_trace, right_trace;
-
- MAKE_LINE(left_trace, lp1, lp2);
- MAKE_LINE(right_trace, rp1, rp2);
-
- // 涓嶅悓鍦板尯宸﹀彸鍊掑簱娆″簭鍙兘涓嶅悓锛屾墍浠ヤ袱涓柟鍚戦兘鍙互杩涘叆
- for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) {
- Line left_entrance_line;
-
- MAKE_LINE(left_entrance_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);
-
- if (IntersectionOf(left_trace, left_entrance_line) == GM_Intersection
- && IntersectionOfLine(lp1, left_entrance_line) == RELATION_RIGHT) {
- return i;
- }
- }
- return -1;
-}
-
-// 杞﹁疆鏄惁鍘嬮亾璺竟绾�
-static bool CrashRedLine1(const Polygon *map, const car_model_t *car)
+static void ParkTimeout(apptimer_var_t val)
{
- bool ret = false;
+ // 瓒呮椂90绉掞紝涓嶅悎鏍�
+ DEBUG("绉诲簱90绉掕秴鏃�");
+ AddExamFault(20402);
+}
- Line red_line;
- const int red_lines[][2] = {{0, 7}, {1, 2}, {5, 6}};
+void TestParkEdge(prime_t &prime)
+{
+ if (prime.examing_area.type != MAP_TYPE_PARK_EDGE)
+ return;
+ // 妫�娴嬪悗杞槸鍚﹂┒杩囧簱浣嶅墠娈�
+ if (!overpass_parkingspace) {
+ overpass_parkingspace = OverpassParkspace(prime);
+ }
+
+ if (prime.pMotion->move == BACKWARD && prime.sensor.gear == GEAR_R) {
+ if (!firstMoveBack) {
+ // 寮�濮嬫寕鍊掓尅鍊掕溅
+ firstMoveBack = true;
+ if (!overpass_parkingspace) {
+ DEBUG("鍚庤疆娌℃湁寮�杩囧簱浣嶇嚎灏卞紑濮嬪�掕溅");
+ AddExamFault(10103);
+ }
+ prime.examing_area.stage = PE_PARKING;
+ AppTimer_delete(ParkTimeout);
+ AppTimer_add(ParkTimeout, prime.examParam.park_edge_limit_time);
+ }
+ }
+
+ //浠庡�掕溅鐘舵�侊紝鐪嬫槸鍚︿粠鍊掓尅閫�鍑�(濡傛灉鍙槸鐪嬪仠杞︾姸鎬侊紝鏃犳硶鍜屼腑閫斿仠杞︽湁鏁堝尯鍒�)
+ if (firstMoveBack && prime.sensor.gear != GEAR_R) {
+ if (!check_parking_space) {
+ check_parking_space = true;
+ if (!CheckParkspace(prime)) {
+ // 鍋滄鍚庯紝杞﹁韩鍑虹嚎锛屼笉鍚堟牸
+ DEBUG("杞﹁韩鍑虹嚎");
+ AddExamFault(20401);
+ }
+ prime.examing_area.stage = PE_EXIT;
+ }
+ }
+
+ // 妫�娴嬪乏杞悜鐏�
+ if (check_parking_space && prime.pMotion->move == FORWARD && !checkLight) {
+ checkLight = true;
+ if (prime.sensor.turn_signal_lamp != LEFT_TURN_LIGHT) {
+ // 涓嶅紑杞悜鐏紝鎵�10鍒�
+ DEBUG("鏈紑鍚浆鍚戠伅");
+ AddExamFault(20405);
+ }
+ }
+
+ if (CrashRedLine(prime)) {
+ if (!occurCrashRedLine) {
+ // 杞﹁韩鍘嬪簱浣嶇嚎锛屾瘡娆℃墸10鍒�
+ DEBUG("杞﹁韩鍘嬪簱浣嶇嚎");
+ AddExamFault(20404);
+ occurCrashRedLine = true;
+ }
+ } else {
+ occurCrashRedLine = false;
+ }
+}
+
+// 杞﹁疆鎴栬溅韬槸鍚﹀帇閬撹矾杈圭嚎
+static bool CrashRedLine(prime_t &prime)
+{
+ const int red_lines[][2] = {{0, 7}, {1, 2}, {2, 3}, {3, 4}, {4, 5}, {5, 6}};
Line frontAxle, rearAxle;
- MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
- MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
+ MAKE_LINE(frontAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]);
+ MAKE_LINE(rearAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);
- for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
- MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]);
- if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
- IntersectionOf(red_line, rearAxle) == GM_Intersection) {
- ret = true;
- break;
- }
- }
-
- return ret;
-}
-
-// 杞﹁韩鏄惁鍘嬪簱浣嶇嚎
-static bool CrashRedLine2(const Polygon *map, const car_model_t *car)
-{
- bool ret = false;
-
- Line red_line;
- const int red_lines[][2] = {{0, 7}, {2, 3}, {3, 4}, {4, 5}};
-
- Polygon car_body;
-
- car_body.num = car->bodyNum;
- car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
- for (int i = 0; i < car_body.num; ++i) {
- car_body.point[i] = car->carXY[car->body[i]];
+ MakePolygon body(prime.pModel->body.size());
+ for (int i = 0; i < prime.pModel->body.size(); ++i) {
+ body.AddPoint(prime.pModeling->points[prime.pModel->body[i]]);
}
for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
- MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]);
- if (IntersectionOf(red_line, &car_body) != GM_None) {
- ret = true;
- break;
- }
- }
-
- free(car_body.point);
- return ret;
-}
-
-static bool CrashRedLine3(const Polygon *map, const car_model_t *car) {
- bool ret = false;
-
- if (!occurMoveBack) {
- // 鍊掕溅鍓嶏紝杞﹁韩涓嶅緱鍘嬪簱浣嶈櫄绾�
- Polygon car_body;
Line red_line;
-
- car_body.num = car->bodyNum;
- car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
- for (int i = 0; i < car_body.num; ++i) {
- car_body.point[i] = car->carXY[car->body[i]];
+ MAKE_LINE(red_line, std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[red_lines[i][0]], std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[red_lines[i][1]]);
+ if (IntersectionOf(red_line, frontAxle) == GM_Intersection
+ || IntersectionOf(red_line, rearAxle) == GM_Intersection
+ || IntersectionOf(red_line, body.GetPolygon()) != GM_None) {
+ return true;
}
-
- MakeLine(&red_line, &map->point[2], &map->point[5]);
- if (IntersectionOf(red_line, &car_body) != GM_None) {
- ret = true;
- }
-
- free(car_body.point);
}
- return ret;
+ return false;
+}
+
+static bool CheckParkspace(prime_t &prime) {
+ DEBUG("妫�鏌ュ仠杞﹀埌浣�...");
+
+ MakePolygon area({std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[8],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[3],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[4],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[9]});
+ MakePolygon car_body(prime.pModel->body.size());
+ for (int i = 0; i < prime.pModel->body.size(); ++i) {
+ car_body.AddPoint(prime.pModeling->points[prime.pModel->body[i]]);
+ }
+
+ return (IntersectionOf(car_body.GetPolygon(), area.GetPolygon()) == GM_Containment)? true : false;
+}
+
+static bool OverpassParkspace(prime_t &prime)
+{
+ Line parkspace_top;
+
+ MAKE_LINE(parkspace_top, std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[4], std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[5]);
+
+ if (IntersectionOfLine(prime.pModeling->points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], parkspace_top) == REL_POS_RIGHT
+ && IntersectionOfLine(prime.pModeling->points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], parkspace_top) == REL_POS_RIGHT) {
+ return true;
+ }
+ return false;
}
diff --git a/lib/src/main/cpp/test_items/park_edge.h b/lib/src/main/cpp/test_items/park_edge.h
index 6fc7e29..06b09e6 100644
--- a/lib/src/main/cpp/test_items/park_edge.h
+++ b/lib/src/main/cpp/test_items/park_edge.h
@@ -7,11 +7,11 @@
#include "../test_common/Geometry.h"
#include "../driver_test.h"
-#include <vector>
-using namespace std;
+void StartParkEdge(prime_t &prime);
-void StartParkEdge(int index, int moveStatus, const struct RtkTime *rtkTime);
-int TestParkEdge(const Polygon *map, const car_model_t *car, const car_model_t *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);
+void TestParkEdge(prime_t &prime);
+
+void StopParkEdge(prime_t &prime);
#endif //RTKDRIVERTEST_PARK_EDGE_H
diff --git a/lib/src/main/cpp/test_items/right_corner.cpp b/lib/src/main/cpp/test_items/right_corner.cpp
new file mode 100644
index 0000000..43dcb9a
--- /dev/null
+++ b/lib/src/main/cpp/test_items/right_corner.cpp
@@ -0,0 +1,137 @@
+//
+// Created by YY on 2019/11/4.
+//
+
+// 4----------------------|5
+// |
+// |
+// 3----------|2 |
+// | |
+// | |
+// | |
+// | |
+// | |
+// |1 |0
+
+#include "right_corner.h"
+#include "../test_common/Geometry.h"
+#include "../driver_test.h"
+#include "../common/apptimer.h"
+#include "../jni_log.h"
+#include "../utils/xconvert.h"
+#include "../defs.h"
+#include "../test_common/car_sensor.h"
+#include "../master/comm_if.h"
+#include "area_exam.h"
+
+#include <tuple>
+#include <vector>
+#include <cstdlib>
+
+#define DEBUG(fmt, args...) LOGD("<turn_a90> <%s>: " fmt, __func__, ##args)
+
+using namespace std;
+
+static int enterAreaHeading;
+static bool turnLeftFinished;
+static bool crashRedLine;
+
+static bool CrashRedLine(prime_t &prime);
+
+void StartRightCorner(prime_t &prime)
+{
+ DEBUG("杩涘叆鐩磋杞集鍦哄湴");
+
+ enterAreaHeading = (int) prime.pModeling->yaw;
+ crashRedLine = false;
+ turnLeftFinished = false;
+
+ MA_EnterMap(prime.examing_area.idx, MAP_TYPE_RIGHT_CORNER, 1);
+}
+
+void StopRightCorner(prime_t &prime)
+{
+ if (prime.examing_area.type != MAP_TYPE_RIGHT_CORNER)
+ return;
+ DEBUG("閫�鍑虹洿瑙掕浆寮満鍦�");
+
+ prime.examing_area.type = MAP_TYPE_NONE;
+}
+
+void TestRightCorner(prime_t &prime)
+{
+ if (prime.examing_area.type != MAP_TYPE_RIGHT_CORNER)
+ return;
+ int az = (int) prime.pModeling->yaw;
+ vector<double> dtox;
+ vector<Line> line_set;
+ Line distance_line;
+
+ if (CrashRedLine(prime)) {
+ if (!crashRedLine) {
+ crashRedLine = true;
+ // 纰惧帇閬撹矾杈圭紭,涓嶅悎鏍�
+ AddExamFault(20701);
+ DEBUG("纰惧帇閬撹矾杈圭紭");
+ }
+ } else {
+ crashRedLine = false;
+ }
+
+ // 妫�鏌ヨ浆鍚戠姸鎬�
+ if (ABS(az - enterAreaHeading) > 180) {
+ az = 360 - ABS(az-enterAreaHeading);
+ } else {
+ az = ABS(az - enterAreaHeading);
+ }
+
+ if (az >= 30) {
+ if (!turnLeftFinished) {
+ char turn_direct;
+ if((( az + 360 - enterAreaHeading) % 360) < 180) {
+ DEBUG("鍙宠浆");
+ turn_direct = 'R';
+ } else {
+ DEBUG("宸﹁浆");
+ turn_direct = 'L';
+ }
+ // 杞悜鐏湭寮�鍚紝鎵�10鍒�
+ if ((turn_direct == 'R' && prime.sensor.turn_signal_lamp != RIGHT_TURN_LIGHT) ||
+ (turn_direct == 'L' && prime.sensor.turn_signal_lamp != LEFT_TURN_LIGHT)) {
+ DEBUG("杞悜鐏湭寮�鍚�");
+ AddExamFault(20702);
+ }
+ }
+ turnLeftFinished = true;
+ }
+}
+
+// 杞﹁疆鏄惁鍘嬭竟绾�
+static bool CrashRedLine(prime_t &prime)
+{
+ bool ret = false;
+
+ const int red_lines[][2] = {{0, 5}, {5, 4}, {1, 2}, {2, 3}};
+
+ Line frontAxle, rearAxle;
+
+ // 浠呯湅杞﹁疆澶栦晶
+ MAKE_LINE(frontAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
+ prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]);
+ MAKE_LINE(rearAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
+ prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);
+
+ for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
+ Line red_line;
+ MAKE_LINE(red_line, std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[red_lines[i][0]],
+ std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[red_lines[i][1]]);
+
+ if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
+ IntersectionOf(red_line, rearAxle) == GM_Intersection) {
+ ret = true;
+ break;
+ }
+ }
+
+ return ret;
+}
diff --git a/lib/src/main/cpp/test_items/right_corner.h b/lib/src/main/cpp/test_items/right_corner.h
new file mode 100644
index 0000000..beae1e9
--- /dev/null
+++ b/lib/src/main/cpp/test_items/right_corner.h
@@ -0,0 +1,15 @@
+//
+// Created by YY on 2019/11/4.
+//
+
+#ifndef RTKDRIVERTEST_TURN_A90_H
+#define RTKDRIVERTEST_TURN_A90_H
+
+#include "../driver_test.h"
+#include <vector>
+
+void StartRightCorner(prime_t &prime);
+void TestRightCorner(prime_t &prime);
+void StopRightCorner(prime_t &prime);
+
+#endif //RTKDRIVERTEST_TURN_A90_H
diff --git a/lib/src/main/cpp/test_items/stop_and_start.h b/lib/src/main/cpp/test_items/stop_and_start.h
deleted file mode 100644
index 220d1a8..0000000
--- a/lib/src/main/cpp/test_items/stop_and_start.h
+++ /dev/null
@@ -1,15 +0,0 @@
-//
-// Created by YY on 2019/10/31.
-//
-
-#ifndef RTKDRIVERTEST_STOP_AND_START_H
-#define RTKDRIVERTEST_STOP_AND_START_H
-
-#include "../driver_test.h"
-
-using namespace std;
-
-void StartSAS(int index, const Polygon *map, const car_model_t *car, int moveDirect, const struct RtkTime *rtkTime);
-int TestSAS(const Polygon *map, const car_model_t *car, const car_model_t *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);
-
-#endif //RTKDRIVERTEST_STOP_AND_START_H
diff --git a/lib/src/main/cpp/test_items/turn_a90.cpp b/lib/src/main/cpp/test_items/turn_a90.cpp
deleted file mode 100644
index 724c69d..0000000
--- a/lib/src/main/cpp/test_items/turn_a90.cpp
+++ /dev/null
@@ -1,191 +0,0 @@
-//
-// Created by YY on 2019/11/4.
-//
-
-// 4----------------------|5
-// |
-// |
-// 3----------|2 |
-// | |
-// | |
-// | |
-// | |
-// | |
-// |1 |0
-
-#include "turn_a90.h"
-#include "../test_common/Geometry.h"
-#include "../driver_test.h"
-#include "../common/apptimer.h"
-#include "../jni_log.h"
-#include "../utils/xconvert.h"
-#include "../defs.h"
-#include "../test_common/car_sensor.h"
-#include "../master/comm_if.h"
-#include "area_exam.h"
-
-#include <vector>
-#include <cstdlib>
-
-#define DEBUG(fmt, args...) LOGD("<turn_a90> <%s>: " fmt, __func__, ##args)
-
-using namespace std;
-
-static bool testing;
-
-static int enterAreaHeading;
-static bool turnLeftFinished;
-static bool crashRedLine;
-
-static bool CrashRedLine(prime_t &prime);
-static bool ExitTestArea(prime_t &prime);
-
-void StartTurnA90(prime_t &prime)
-{
- DEBUG("杩涘叆鐩磋杞集鍦哄湴");
- testing = true;
- enterAreaHeading = (int) prime.pModeling->yaw;
- crashRedLine = false;
- turnLeftFinished = false;
-
- MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_TURN_90, 1);
-}
-
-static void StoppingTimeout(apptimer_var_t val) {
- DEBUG("涓�斿仠杞�");
- AddExamFault(20703);
-}
-
-void MotionChange(move_status_t mv)
-{
- if (!testing)
- return;
-
- AppTimer_delete(StoppingTimeout);
-
- if (mv == STOP) {
- AppTimer_add(StoppingTimeout, D_SEC(2));
- } else {
-
- }
-}
-
-void TestTurnA90(prime_t &prime)
-{
- int az = (int) prime.pModeling->yaw;
- vector<double> dtox;
- vector<Line> line_set;
- Line distance_line;
-
- if (CrashRedLine(prime)) {
- if (!crashRedLine) {
- crashRedLine = true;
- // 纰惧帇閬撹矾杈圭紭,涓嶅悎鏍�
- AddExamFault(20701);
- DEBUG("纰惧帇閬撹矾杈圭紭");
- }
- } else {
- crashRedLine = false;
- }
-
- // 妫�鏌ヨ浆鍚戠姸鎬�
- if (ABS(az - enterAreaHeading) > 180) {
- az = 360 - ABS(az-enterAreaHeading);
- } else {
- az = ABS(az - enterAreaHeading);
- }
-
- if (az >= 30) {
- if (!turnLeftFinished) {
- char turn_direct;
- if((( az + 360 - enterAreaHeading) % 360) < 180) {
- DEBUG("鍙宠浆");
- turn_direct = 'R';
- } else {
- DEBUG("宸﹁浆");
- turn_direct = 'L';
- }
- // 杞悜鐏湭寮�鍚紝鎵�10鍒�
- if ((turn_direct == 'R' && ReadCarStatus(TURN_SIGNAL_LAMP) != RIGHT_TURN_LIGHT) ||
- (turn_direct == 'L' && ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT)) {
- DEBUG("杞悜鐏湭寮�鍚�");
- AddExamFault(20702);
- }
- }
- turnLeftFinished = true;
- }
-
- if (ExitTestArea(prime)) {
- testing = false;
- MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_TURN_90, 0);
- }
-}
-
-// 杞﹁疆鏄惁鍘嬭竟绾�
-static bool CrashRedLine(prime_t &prime)
-{
- bool ret = false;
-
- const int red_lines[][2] = {{0, 5}, {5, 4}, {1, 2}, {2, 3}};
-
- Line frontAxle, rearAxle;
-
- // 浠呯湅杞﹁疆澶栦晶
- MAKE_LINE(frontAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
- prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]);
- MAKE_LINE(rearAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
- prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);
-
- for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
- Line red_line;
- MAKE_LINE(red_line, prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map[red_lines[i][0]],
- prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map[red_lines[i][1]]);
-
- if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
- IntersectionOf(red_line, rearAxle) == GM_Intersection) {
- ret = true;
- break;
- }
- }
-
- return ret;
-}
-
-// 4涓溅杞拰杞﹀ご鐐逛笉鍦ㄥ満鍦颁腑
-static bool ExitTestArea(prime_t &prime)
-{
- Polygon polygon;
-
- polygon.num = prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map.size();
- polygon.point = new PointF[polygon.num];
- for (int i = 0; i < polygon.num; ++i) {
- polygon.point[i] = prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map[i];
- }
-
- int num = 0;
-
- if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
- &polygon) == GM_None) {
- num++;
- }
- if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
- &polygon) == GM_None) {
- num++;
- }
- if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
- &polygon) == GM_None) {
- num++;
- }
- if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]],
- &polygon) == GM_None) {
- num++;
- }
- if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[prime.pModel->axial[AXIAL_FRONT]]],
- &polygon) == GM_None) {
- num++;
- }
-
- delete []polygon.point;
-
- return num == 5? true : false;
-}
diff --git a/lib/src/main/cpp/test_items/turn_a90.h b/lib/src/main/cpp/test_items/turn_a90.h
deleted file mode 100644
index f82df8b..0000000
--- a/lib/src/main/cpp/test_items/turn_a90.h
+++ /dev/null
@@ -1,16 +0,0 @@
-//
-// Created by YY on 2019/11/4.
-//
-
-#ifndef RTKDRIVERTEST_TURN_A90_H
-#define RTKDRIVERTEST_TURN_A90_H
-
-#include "../driver_test.h"
-#include <vector>
-
-using namespace std;
-
-void StartTurnA90(int index, int moveDirect, double heading, const struct RtkTime *rtkTime);
-int TestTurnA90(const Polygon *map, const car_model_t *car, const car_model_t *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime);
-
-#endif //RTKDRIVERTEST_TURN_A90_H
diff --git a/lib/src/main/cpp/test_items/stop_and_start.cpp b/lib/src/main/cpp/test_items/uphill.cpp
similarity index 78%
rename from lib/src/main/cpp/test_items/stop_and_start.cpp
rename to lib/src/main/cpp/test_items/uphill.cpp
index 0037350..6d0c532 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.cpp
+++ b/lib/src/main/cpp/test_items/uphill.cpp
@@ -2,7 +2,7 @@
// Created by YY on 2019/10/31.
//
-// |9
+// 11 |9
// |
// |
// |
@@ -15,13 +15,14 @@
// |
// |
// |
-// |0
+// 10 |0
#include <cstdlib>
#include <vector>
#include <cmath>
+#include <tuple>
-#include "stop_and_start.h"
+#include "uphill.h"
#include "../driver_test.h"
#include "../jni_log.h"
#include "../common/apptimer.h"
@@ -35,13 +36,9 @@
using namespace std;
-
-
-
const double EPSILON = 1e-3;
-static bool testing = false;
-
+static int handle;
static PointF stopPoint;
static bool check1 = false;
@@ -58,12 +55,12 @@
static double DistanceOfHead2Stopline(prime_t &prime);
static double DistanceOfTire2Edge(prime_t &prime);
static bool ExitTestArea(prime_t &prime);
+static void MotionChange(move_status_t mv, move_status_t prev, double distance);
-void StartSAS(prime_t &prime)
+void StartUphill(prime_t &prime)
{
DEBUG("杩涘叆鍧¤捣椤圭洰");
- testing = true;
stopConfirm = false;
restartComplete = false;
occurCrashRedLine = false;
@@ -71,8 +68,24 @@
slideNormalDistance = false;
reportSlideFault = false;
handBreakActive = false;
+ handle = RegisterCarMoveObserver(MotionChange);
+ MA_EnterMap(prime.examing_area.idx, MAP_TYPE_UPHILL, 1);
+}
- MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_UPHILL, 1);
+void StopUphill(prime_t &prime)
+{
+ if (prime.examing_area.type != MAP_TYPE_UPHILL)
+ return;
+ DEBUG("閫�鍑哄潯璧烽」鐩�");
+
+ if (!check1) {
+ DEBUG("鏈繘琛屽仠杞﹁捣姝ュ姩浣�");
+ AddExamFault(10103);
+ }
+
+ UnregisterCarMoveObserver(handle);
+
+ prime.examing_area.type = MAP_TYPE_NONE;
}
static void StoppingTimeout(apptimer_var_t val) {
@@ -85,7 +98,7 @@
stopConfirm = true;
}
-void MotionChange(move_status_t mv)
+static void MotionChange(move_status_t mv, move_status_t prev, double distance)
{
AppTimer_delete(StopConfirm);
@@ -96,8 +109,10 @@
}
}
-int TestSAS(prime_t &prime)
+void TestUphill(prime_t &prime)
{
+ if (prime.examing_area.type != MAP_TYPE_UPHILL)
+ return;
static double distanceToStopLine = 0, distanceToEdge = 0;
if (CrashRedLine(prime)) {
@@ -119,7 +134,7 @@
stopPoint = prime.pModeling->base_point;
// 寮�濮嬪仠杞﹁鏃�
AppTimer_delete(StoppingTimeout);
- AppTimer_add(StoppingTimeout, examParam.ramp_start_car_limit_time);
+ AppTimer_add(StoppingTimeout, prime.examParam.ramp_start_car_limit_time);
// 妫�鏌ヨ溅澶村拰鍋滆溅甯︾殑璺濈
distanceToStopLine = DistanceOfHead2Stopline(prime);
@@ -127,7 +142,7 @@
DEBUG("DIS1 = %f DIS2 = %f", distanceToStopLine, distanceToEdge);
- if (distanceToStopLine > examParam.ramp_stoppoint_red_distance) {
+ if (distanceToStopLine > prime.examParam.ramp_stoppoint_red_distance) {
// 璺濈鍋滄绾垮墠鍚庤秴鍑�50鍘樼背
AddExamFault(20301);
DEBUG("璺濈鍋滄绾垮墠鍚庤秴鍑�50鍘樼背锛屼笉鍚堟牸");
@@ -137,11 +152,11 @@
DEBUG("鍓嶄繚闄╂病鏈変綅浜庡仠姝㈠甫鍐咃紝浣嗘病鏈夎秴鍑�50鍘樼背");
}
- if (distanceToEdge > examParam.ramp_edge_red_distance) {
+ if (distanceToEdge > prime.examParam.ramp_edge_red_distance) {
// 璺濈杈圭嚎瓒呭嚭50鍘樼背,涓嶅悎鏍�
AddExamFault(20302);
DEBUG("璺濈杈圭嚎瓒呭嚭50鍘樼背");
- } else if (distanceToEdge > examParam.ramp_edge_yellow_distance) {
+ } else if (distanceToEdge > prime.examParam.ramp_edge_yellow_distance) {
// 璺濈杈圭嚎瓒呭嚭30鍘樼背锛屾墸10鍒�
AddExamFault(20305);
DEBUG("璺濈杈圭嚎瓒呭嚭30鍘樼背");
@@ -149,7 +164,7 @@
}
// 鍋滆溅鍚庯紝妫�鏌ユ墜鍒规媺璧锋儏鍐�
- if (!handBreakActive && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
+ if (!handBreakActive && prime.sensor.hand_brake == ACTIVE) {
handBreakActive = true;
}
}
@@ -158,7 +173,7 @@
// 杞﹁締浠庡仠姝㈢姸鎬佸啀娆＄Щ鍔�
double juli = DistanceOf(prime.pModeling->base_point, stopPoint);
- if (juli > examParam.ramp_slide_yellow_distance) {
+ if (juli > prime.examParam.ramp_slide_yellow_distance) {
double deg = YawOf(stopPoint, prime.pModeling->base_point);
deg = fabs(prime.pModeling->yaw - deg);
if (deg > 180) {
@@ -185,7 +200,7 @@
} else {
// 杞﹁締鍚庢粦
slideNormalDistance = true;
- if (juli > examParam.ramp_slide_red_distance && !reportSlideFault) {
+ if (juli > prime.examParam.ramp_slide_red_distance && !reportSlideFault) {
// 鍚庢粦瓒呰繃30鍘樼背, 涓嶅悎鏍�
DEBUG("鍚庢粦瓒呰繃30鍘樼背");
reportSlideFault = true;
@@ -201,10 +216,8 @@
// 涓嶅仠杞︾洿鎺ョ寮�
AddExamFault(10103);
}
- MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_UPHILL, 0);
+ MA_EnterMap(prime.examing_area.idx, MAP_TYPE_UPHILL, 0);
}
-
- return testing ? 1 : 0;
}
// 杞﹁疆鏄惁鍘嬭竟绾�
@@ -225,8 +238,8 @@
for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
Line red_line;
- MAKE_LINE(red_line, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[red_lines[i][0]],
- prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[red_lines[i][1]]);
+ MAKE_LINE(red_line, std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[red_lines[i][0]],
+ std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[red_lines[i][1]]);
if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
IntersectionOf(red_line, rearAxle) == GM_Intersection) {
@@ -244,10 +257,10 @@
Line upper_edge, lower_edge;
- MAKE_LINE(upper_edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[5],
- prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[6]);
- MAKE_LINE(lower_edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[4],
- prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[3]);
+ MAKE_LINE(upper_edge, std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[5],
+ std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[6]);
+ MAKE_LINE(lower_edge, std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[4],
+ std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[3]);
if (IntersectionOfLine(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], upper_edge) == REL_POS_RIGHT) {
dis = DistanceOf(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], upper_edge);
@@ -264,8 +277,8 @@
{
Line edge;
- MAKE_LINE(edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[0],
- prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[8]);
+ MAKE_LINE(edge, std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[0],
+ std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[8]);
double l1 = DistanceOf(prime.pModeling->points[prime.pModel->right_front_tire[TIRE_OUTSIDE]], edge);
diff --git a/lib/src/main/cpp/test_items/uphill.h b/lib/src/main/cpp/test_items/uphill.h
new file mode 100644
index 0000000..027beab
--- /dev/null
+++ b/lib/src/main/cpp/test_items/uphill.h
@@ -0,0 +1,14 @@
+//
+// Created by YY on 2019/10/31.
+//
+
+#ifndef RTKDRIVERTEST_STOP_AND_START_H
+#define RTKDRIVERTEST_STOP_AND_START_H
+
+#include "../driver_test.h"
+
+void StartUphill(prime_t &prime);
+void TestUphill(prime_t &prime);
+void StopUphill(prime_t &prime);
+
+#endif //RTKDRIVERTEST_STOP_AND_START_H
diff --git a/lib/src/main/java/com/anyun/exam/lib/ActionManager.kt b/lib/src/main/java/com/anyun/exam/lib/ActionManager.kt
new file mode 100644
index 0000000..48b1e96
--- /dev/null
+++ b/lib/src/main/java/com/anyun/exam/lib/ActionManager.kt
@@ -0,0 +1,39 @@
+package com.anyun.exam.lib
+
+import android.content.Context
+import com.anyun.exam.lib.net.NtripTcpClient
+import java.lang.Exception
+import java.util.concurrent.Executors
+
+class ActionManager {
+ val actionList = mutableListOf<BaseAction>()
+ var executor = Executors.newFixedThreadPool(10)
+
+ init {
+ actionList.add(ActionRtcm())
+ }
+
+ fun execute(args: ActionArgs)
+ {
+ //浣跨敤绾跨▼姹犲鐞�
+ executor.submit{
+ actionList.forEach {
+ try {
+ it.execute(args)
+ }
+ catch (ex: Exception)
+ {
+
+ }
+ }
+ }
+
+ }
+}
+
+class ActionArgs
+{
+ lateinit var message:ByteArray
+ lateinit var context: Context
+ lateinit var tcpClient: NtripTcpClient
+}
\ No newline at end of file
diff --git a/lib/src/main/java/com/anyun/exam/lib/ActionRtcm.kt b/lib/src/main/java/com/anyun/exam/lib/ActionRtcm.kt
new file mode 100644
index 0000000..c78492a
--- /dev/null
+++ b/lib/src/main/java/com/anyun/exam/lib/ActionRtcm.kt
@@ -0,0 +1,7 @@
+package com.anyun.exam.lib
+
+class ActionRtcm: BaseAction {
+ override fun execute(args: ActionArgs) {
+ Rtcm.sendRtcm(args.message)
+ }
+}
\ No newline at end of file
diff --git a/lib/src/main/java/com/anyun/exam/lib/BaseAction.kt b/lib/src/main/java/com/anyun/exam/lib/BaseAction.kt
new file mode 100644
index 0000000..73570f2
--- /dev/null
+++ b/lib/src/main/java/com/anyun/exam/lib/BaseAction.kt
@@ -0,0 +1,5 @@
+package com.anyun.exam.lib
+
+interface BaseAction {
+ fun execute(args:ActionArgs)
+}
\ No newline at end of file
diff --git a/lib/src/main/java/com/anyun/exam/lib/IRtcmData.kt b/lib/src/main/java/com/anyun/exam/lib/IRtcmData.kt
new file mode 100644
index 0000000..33008dd
--- /dev/null
+++ b/lib/src/main/java/com/anyun/exam/lib/IRtcmData.kt
@@ -0,0 +1,5 @@
+package com.anyun.exam.lib
+
+interface IRtcmData {
+ fun sendRtcm(data: ByteArray, length: Int)
+}
\ No newline at end of file
diff --git a/lib/src/main/java/com/anyun/exam/lib/RemoteService.java b/lib/src/main/java/com/anyun/exam/lib/RemoteService.java
index 83052a5..7278f45 100644
--- a/lib/src/main/java/com/anyun/exam/lib/RemoteService.java
+++ b/lib/src/main/java/com/anyun/exam/lib/RemoteService.java
@@ -23,6 +23,9 @@
import android.util.Base64;
import android.util.Log;
+import com.anyun.exam.lib.net.Constant;
+import com.anyun.exam.lib.net.NtripTcpClient;
+import com.anyun.exam.lib.net.TcpSession;
import com.anyun.exam.lib.util.Bluetooth;
import com.anyun.exam.lib.util.BluetoothChatService;
import com.anyun.exam.lib.util.BluetoothChatServiceCallback;
@@ -55,7 +58,7 @@
* 閭锛�632393724@qq.com
* All Rights Saved! Chongqing AnYun Tech co. LTD
*/
-public class RemoteService extends Service {
+public class RemoteService extends Service implements IRtcmData {
private static final String TAG = "RemoteService";
public static final boolean mAyDevice = true;
@@ -80,6 +83,8 @@
private String mTargetBluetooth = null;
private String mTargetBluetoothAddr = null;
private boolean mDiscoveryBluetooth = false;
+
+ public NtripTcpClient ntripTcpClient = null;
private IRemoteInterface.Stub iRemoteInterface = new IRemoteInterface.Stub(){
@Override
@@ -150,6 +155,16 @@
String ver = getBaseband_Ver();
new Thread(new TestCls()).start();
+
+ Rtcm.INSTANCE.registerCallback(this);
+ TcpSession tcpSession = new TcpSession();
+ tcpSession.ip = "base.staroadnav.com";
+// tcpSession.ip = "192.168.16.212";
+ tcpSession.phone = "";
+ tcpSession.setPort(6060);
+// tcpSession.setPort(8080);
+ ntripTcpClient = new NtripTcpClient(tcpSession);
+ ntripTcpClient.connect(this);
}
class TestCls implements Runnable {
@@ -302,6 +317,8 @@
public void onDestroy() {
super.onDestroy();
Log.i(TAG,"onDestroy");
+
+ Rtcm.INSTANCE.unRegisterCallback();
if (mChatService != null) {
mChatService.stop();
@@ -569,6 +586,11 @@
}
}
+ @Override
+ public void sendRtcm(byte []data, int length) {
+ uploadRtcm(data, length);
+ }
+
// Used to load the 'native-lib' library on application startup.
static {
System.loadLibrary("native-lib");
@@ -582,4 +604,5 @@
public native void BluetoothConnected(String name, String addr);
public native void BluetoothStatusChange(int status);
public native void BluetoothDataComeIn(byte []data, int length);
+ public native void uploadRtcm(byte []data, int length);
}
diff --git a/lib/src/main/java/com/anyun/exam/lib/Rtcm.kt b/lib/src/main/java/com/anyun/exam/lib/Rtcm.kt
new file mode 100644
index 0000000..2bdfc43
--- /dev/null
+++ b/lib/src/main/java/com/anyun/exam/lib/Rtcm.kt
@@ -0,0 +1,17 @@
+package com.anyun.exam.lib
+
+object Rtcm {
+ private var callback: IRtcmData? = null
+
+ fun registerCallback(cb: IRtcmData) {
+ callback = cb
+ }
+
+ fun unRegisterCallback() {
+ callback = null
+ }
+
+ fun sendRtcm(data: ByteArray, length: Int = data.size) {
+ callback?.sendRtcm(data, length)
+ }
+}
\ No newline at end of file
diff --git a/lib/src/main/java/com/anyun/exam/lib/net/ConnectionWatchdog.kt b/lib/src/main/java/com/anyun/exam/lib/net/ConnectionWatchdog.kt
new file mode 100644
index 0000000..a6eed8c
--- /dev/null
+++ b/lib/src/main/java/com/anyun/exam/lib/net/ConnectionWatchdog.kt
@@ -0,0 +1,42 @@
+package com.anyun.exam.lib.net
+
+import android.util.Log
+import io.netty.channel.ChannelHandler
+import io.netty.channel.ChannelHandlerContext
+import io.netty.channel.ChannelInboundHandlerAdapter
+import io.netty.util.AttributeKey
+
+
+
+@ChannelHandler.Sharable
+class ConnectionWatchdog : ChannelInboundHandlerAdapter() {
+ internal var KEY = AttributeKey.valueOf<NettyTcpClient>("TcpClient")
+ /**
+ * channel閾捐矾姣忔active鐨勬椂鍊欙紝灏嗗叾杩炴帴鐨勬鏁伴噸鏂扮疆0
+ */
+ override fun channelActive(ctx: ChannelHandlerContext) {
+ val tcpClient = ctx.channel().attr(KEY).get()
+ Log.i(Constant.LOGTAG, "TCP宸茬粡杩炴帴鑷砙${tcpClient.host}:${tcpClient.port}]")
+ tcpClient.autoConnect = true
+ tcpClient.isConnected = true
+ tcpClient.isAuthentication = false
+ tcpClient.dataStatus = NettyTcpClient.WAIT_AUTH
+ ctx.fireChannelActive()
+ }
+
+ override fun channelInactive(ctx: ChannelHandlerContext) {
+ val tcpClient = ctx.channel().attr(KEY).get()
+ tcpClient.isConnected = false
+ ctx.fireChannelInactive()
+ if(tcpClient.autoConnect) {
+ Log.i(Constant.LOGTAG, "TCP閾炬帴宸叉柇寮�[${tcpClient.host}:${tcpClient.port}] 2绉掑悗閲嶆柊杩炴帴")
+ Thread.sleep(2000)
+ tcpClient.connect(tcpClient.context)
+ }
+ else
+ {
+ Log.i(Constant.LOGTAG, "TCP閾炬帴宸叉柇寮�[${tcpClient.host}:${tcpClient.port}] 闇�瑕佹墜鍔ㄩ噸鏂拌繛鎺�")
+ }
+ }
+}
+
diff --git a/lib/src/main/java/com/anyun/exam/lib/net/ConnectorIdleStateTrigger.kt b/lib/src/main/java/com/anyun/exam/lib/net/ConnectorIdleStateTrigger.kt
new file mode 100644
index 0000000..05184d4
--- /dev/null
+++ b/lib/src/main/java/com/anyun/exam/lib/net/ConnectorIdleStateTrigger.kt
@@ -0,0 +1,32 @@
+package com.anyun.exam.lib.net
+
+import android.util.Log
+import io.netty.channel.ChannelHandler
+import io.netty.channel.ChannelHandlerContext
+import io.netty.channel.ChannelInboundHandlerAdapter
+import io.netty.handler.timeout.IdleState
+import io.netty.handler.timeout.IdleStateEvent
+import io.netty.util.AttributeKey
+
+@ChannelHandler.Sharable
+class ConnectorIdleStateTrigger : ChannelInboundHandlerAdapter() {
+ internal var KEY = AttributeKey.valueOf<NtripTcpClient>("TcpClient")
+
+ override fun userEventTriggered(ctx: ChannelHandlerContext, evt: Any) {
+ if (evt is IdleStateEvent) {
+ val tcpClient = ctx.channel().attr(KEY).get()
+ val state = evt.state()
+// Log.d(Constant.LOGTAG, "tcp idle =$state")
+ if(state == IdleState.WRITER_IDLE) {
+// Log.d(Constant.LOGTAG,"瓒呮椂鑷姩鍏抽棴杩炴帴 ${tcpClient.session.ip}:${tcpClient.session.port}")
+ }
+ if(state == IdleState.READER_IDLE) {
+// Log.d(Constant.LOGTAG,"瓒呮椂鑷姩鍏抽棴杩炴帴 ${tcpClient.session.ip}:${tcpClient.session.port}")
+ tcpClient.close(true)
+ }
+ } else {
+ super.userEventTriggered(ctx, evt)
+ }
+ }
+}
+
diff --git a/lib/src/main/java/com/anyun/exam/lib/net/Constant.kt b/lib/src/main/java/com/anyun/exam/lib/net/Constant.kt
new file mode 100644
index 0000000..e4229fe
--- /dev/null
+++ b/lib/src/main/java/com/anyun/exam/lib/net/Constant.kt
@@ -0,0 +1,8 @@
+package com.anyun.exam.lib.net
+
+class Constant {
+ companion object
+ {
+ val LOGTAG = "ntrip"
+ }
+}
\ No newline at end of file
diff --git a/lib/src/main/java/com/anyun/exam/lib/net/ExceptionHandler.kt b/lib/src/main/java/com/anyun/exam/lib/net/ExceptionHandler.kt
new file mode 100644
index 0000000..02d7c7d
--- /dev/null
+++ b/lib/src/main/java/com/anyun/exam/lib/net/ExceptionHandler.kt
@@ -0,0 +1,24 @@
+package com.anyun.exam.lib.net
+
+import android.util.Log
+import io.netty.channel.ChannelPromise
+import io.netty.channel.ChannelDuplexHandler
+import io.netty.channel.ChannelFutureListener
+import io.netty.channel.ChannelHandlerContext
+
+
+class ExceptionHandler : ChannelDuplexHandler() {
+ override fun exceptionCaught(ctx: ChannelHandlerContext, cause: Throwable) {
+ // Uncaught exceptions from inbound handlers will propagate up to this handler
+ Log.e(Constant.LOGTAG,cause.localizedMessage)
+ }
+// @Throws(Exception::class)
+// override fun write(ctx: ChannelHandlerContext, msg: Any, promise: ChannelPromise) {
+// ctx.write(msg, promise.addListener { future ->
+// if (!future.isSuccess) {
+// logger.error("send data to client exception occur: ", future.cause())
+// }
+// })
+// }
+
+}
\ No newline at end of file
diff --git a/lib/src/main/java/com/anyun/exam/lib/net/MessageHandler.kt b/lib/src/main/java/com/anyun/exam/lib/net/MessageHandler.kt
new file mode 100644
index 0000000..46f988e
--- /dev/null
+++ b/lib/src/main/java/com/anyun/exam/lib/net/MessageHandler.kt
@@ -0,0 +1,80 @@
+package com.anyun.exam.lib.net
+
+import android.os.SystemClock
+import android.util.Log
+import com.anyun.exam.lib.Rtcm
+import com.anyun.exam.lib.util.ByteUtil
+import io.netty.channel.ChannelHandlerContext
+import io.netty.channel.SimpleChannelInboundHandler
+import io.netty.util.AttributeKey
+
+
+
+class MessageHandler : SimpleChannelInboundHandler<ByteArray>() {
+ val KEY = AttributeKey.valueOf<NtripTcpClient>("TcpClient")
+
+// private var actionManager = ActionManager()
+ private val parser = ParseNtripPackage()
+
+ override fun channelRead0(ctx: ChannelHandlerContext, message: ByteArray) {
+// val message = msg as JT808_2016_MessageBase
+ val tcpClient = ctx.channel().attr(KEY).get()
+
+// Log.d(Constant.LOGTAG,"${Thread.currentThread().id} 鏀跺埌鏁版嵁<${tcpClient.host}:${tcpClient.port}>锛� ${message.size} - ${ByteUtil.byte2hex(message)}")
+
+ if (tcpClient.dataStatus == NettyTcpClient.WAIT_AUTH) {
+ val httpHead = String(message)
+ Log.d(Constant.LOGTAG, "http head: ${httpHead}")
+
+ if (httpHead.startsWith("HTTP/1.1 200 OK") || httpHead.startsWith("ICY 200 OK")) {
+ Log.d(Constant.LOGTAG, "閴存潈鎴愬姛")
+ tcpClient.isAuthentication = true
+ } else if (httpHead.startsWith("Error")) {
+ tcpClient.isAuthentication = false
+ }
+ }
+ if (message.size == 2) {
+ if (tcpClient.isAuthentication) {
+ Log.d(Constant.LOGTAG, "寮�濮嬫帴鏀禦TCM鏁版嵁")
+ tcpClient.dataStatus = NettyTcpClient.BODY
+ parser.reset()
+ } else {
+ tcpClient.close(true)
+ }
+ }
+ if (tcpClient.dataStatus == NettyTcpClient.BODY) {
+ parser.parse(tcpClient, message)
+// Rtcm.sendRtcm(message)
+ }
+ }
+
+// private var recvExecutorService: TcpRecvAction = SpringContextUtil.getBean(TcpRecvAction::class.java)
+
+ override fun channelActive(ctx: ChannelHandlerContext) {
+ val tcpClient = ctx.channel().attr(KEY).get()
+ Log.d(Constant.LOGTAG, "channelActive")
+ //閴存潈鏍囧織鏀逛负false
+ tcpClient.isAuthentication = false
+ tcpClient.dataStatus = NettyTcpClient.WAIT_AUTH
+
+ tcpClient.sendNtripRequest("Demo", "YJP_500100_ANYUN", "adm", "adm")
+ }
+
+ override fun channelInactive(ctx: ChannelHandlerContext) {
+ val tcpClient = ctx.channel().attr(KEY).get()
+ Log.d(Constant.LOGTAG, "channelInactive")
+ }
+// override fun channelRead(ctx: ChannelHandlerContext, msg: Any) {
+// val message = msg as JT808_2016_MessageBase
+// val tcpClient = ctx.channel().attr(KEY).get()
+// Timber.d( "鏀跺埌鏁版嵁:${Int16Coder.instance.encode(message.messageId)}")
+// //鍚庣画澶勭悊杩囩▼
+// actionManager.execute(ActionArgs().apply {
+// this.context = tcpClient.context
+// this.message = message
+// this.tcpClient = tcpClient
+// })
+//
+// }
+
+}
diff --git a/lib/src/main/java/com/anyun/exam/lib/net/NettyTcpClient.kt b/lib/src/main/java/com/anyun/exam/lib/net/NettyTcpClient.kt
new file mode 100644
index 0000000..6b1c5cc
--- /dev/null
+++ b/lib/src/main/java/com/anyun/exam/lib/net/NettyTcpClient.kt
@@ -0,0 +1,134 @@
+package com.anyun.exam.lib.net
+
+import android.content.Context
+import android.util.Log
+import io.netty.bootstrap.Bootstrap
+import io.netty.buffer.Unpooled
+import io.netty.channel.Channel
+import io.netty.channel.ChannelFutureListener
+import io.netty.channel.ChannelInitializer
+import io.netty.channel.ChannelOption
+import io.netty.channel.nio.NioEventLoopGroup
+import io.netty.channel.socket.nio.NioSocketChannel
+import io.netty.handler.codec.DelimiterBasedFrameDecoder
+import io.netty.handler.logging.LogLevel
+import io.netty.handler.logging.LoggingHandler
+import io.netty.handler.timeout.IdleStateHandler
+import io.netty.util.AttributeKey
+import java.util.concurrent.TimeUnit
+
+abstract class NettyTcpClient(var session: TcpSession) {
+ val KEY = AttributeKey.valueOf<NettyTcpClient>("TcpClient")
+ private var bootstrap: Bootstrap = Bootstrap()
+ private val group = NioEventLoopGroup()
+
+ var isAuthentication = false
+ var dataStatus = WAIT_AUTH
+ /**
+ * tcp閾炬帴鎴愬姛鏍囧織
+ */
+ var isConnected = false
+ lateinit var host: String
+ var port: Int = 0
+ var autoConnect = false
+ lateinit var context: Context
+ internal var channel: Channel? = null
+
+ companion object {
+ public val WAIT_AUTH = 0
+ public val BODY = 1
+ }
+
+ init {
+ bootstrap.group(group)
+ .channel(NioSocketChannel::class.java)
+ .option(ChannelOption.SO_KEEPALIVE, true)
+ .handler(LoggingHandler(LogLevel.INFO))
+
+ bootstrap.handler(object : ChannelInitializer<Channel>() {
+ //鍒濆鍖朿hannel
+ override fun initChannel(ch: Channel) {
+ ch.pipeline().addLast(
+ DelimiterBasedFrameDecoder(
+ 2048,
+ false,
+ Unpooled.copiedBuffer(byteArrayOf(0x0D, 0x0A))
+ )
+ )
+ ch.pipeline().addLast(IdleStateHandler(60, 30, 0, TimeUnit.SECONDS))
+ ch.pipeline().addLast(ConnectorIdleStateTrigger())
+ ch.pipeline().addLast(ConnectionWatchdog())
+ ch.pipeline().addLast(NtripDecoder())
+ ch.pipeline().addLast(NtripEncoder())
+ ch.pipeline().addLast(MessageHandler())
+ ch.pipeline().addLast(ExceptionHandler())
+ }
+ })
+ }
+
+ fun connect(context: Context) {
+ this.context = context
+ this.connect(session.ip, session.port)
+ }
+
+ @Synchronized private fun connect(host: String, port: Int) {
+ Log.i(Constant.LOGTAG, "杩炴帴TCP ${host}:${port} ${KEY.name()}...")
+ this.host = host
+ this.port = port
+
+ if(channel!=null && channel!!.isOpen)
+ {
+ Log.i(Constant.LOGTAG, "Close鍏堝墠鐨勮繛鎺�")
+ close(false)
+ }
+ var future = bootstrap.connect(host, port)
+ future.addListener(CONNECT_END_HANDLER)
+ channel = future.channel()
+ }
+
+ private val CONNECT_END_HANDLER = ChannelFutureListener {
+ if (!it.isSuccess) {
+ Log.i(Constant.LOGTAG, "杩炴帴鏈嶅姟鍣ㄥけ璐�$host:$port 绛夊緟5绉掗噸鏂拌繛鎺�")
+ it.channel().pipeline().fireExceptionCaught(it.cause())
+ Thread.sleep(5000)
+
+ if (!isConnected) {
+ this.connect(this.context)
+ } else {
+
+ }
+ }
+ else
+ {
+ isConnected = true
+ it.channel().attr(KEY).set(this)
+ }
+ }
+
+ fun close(auto_connect: Boolean) {
+ try {
+ autoConnect = auto_connect;
+ if(channel!=null && channel!!.isOpen) {
+ channel!!.close();
+ //杩欏彞涓�鐩撮樆濉� 鍘熷洜鏈煡
+ //closeFuture().sync();
+ }
+ }
+ catch (ex: Exception)
+ {
+
+ }
+ }
+
+ fun writeAndFlush(cmdstr: String): Boolean {
+ if (!isConnected) {
+ Log.d(Constant.LOGTAG, "TCP鏈繛鎺�")
+ return false
+ }
+
+ System.out.println("鍙戦�佹暟鎹�<${host}:${port}> = " + cmdstr)
+
+ channel!!.writeAndFlush(cmdstr)
+ return true
+ }
+}
\ No newline at end of file
diff --git a/lib/src/main/java/com/anyun/exam/lib/net/NtripDecoder.kt b/lib/src/main/java/com/anyun/exam/lib/net/NtripDecoder.kt
new file mode 100644
index 0000000..9c05792
--- /dev/null
+++ b/lib/src/main/java/com/anyun/exam/lib/net/NtripDecoder.kt
@@ -0,0 +1,22 @@
+package com.anyun.exam.lib.net
+
+import android.util.Log
+import io.netty.buffer.ByteBuf
+import io.netty.channel.ChannelHandlerContext
+import io.netty.handler.codec.MessageToMessageDecoder
+import io.netty.util.AttributeKey
+
+class NtripDecoder : MessageToMessageDecoder<ByteBuf>() {
+ val KEY = AttributeKey.valueOf<NtripTcpClient>("TcpClient")
+
+ override fun decode(ctx: ChannelHandlerContext, byteBuf: ByteBuf, out: MutableList<Any>) {
+ val tcpClient = ctx.channel().attr(KEY).get()
+ val readLen = byteBuf.readableBytes()
+
+ val req = ByteArray(readLen)
+ byteBuf.readBytes(req, 0, readLen)
+// req[readLen] = 0x0D
+// req[readLen + 1] = 0x0A
+ out.add(req)
+ }
+}
\ No newline at end of file
diff --git a/lib/src/main/java/com/anyun/exam/lib/net/NtripEncoder.kt b/lib/src/main/java/com/anyun/exam/lib/net/NtripEncoder.kt
new file mode 100644
index 0000000..6ebcd48
--- /dev/null
+++ b/lib/src/main/java/com/anyun/exam/lib/net/NtripEncoder.kt
@@ -0,0 +1,15 @@
+package com.anyun.exam.lib.net
+
+import io.netty.buffer.Unpooled
+import io.netty.channel.ChannelHandlerContext
+import io.netty.handler.codec.MessageToMessageEncoder
+
+class NtripEncoder : MessageToMessageEncoder<CharSequence>(){
+ override fun encode(ctx: ChannelHandlerContext, msg: CharSequence, out: MutableList<Any>) {
+ if (msg.isNotEmpty()) {
+ val bytes = msg.toString().toByteArray()
+
+ out.add(Unpooled.copiedBuffer(bytes, 0, bytes.size))
+ }
+ }
+}
\ No newline at end of file
diff --git a/lib/src/main/java/com/anyun/exam/lib/net/NtripTcpClient.kt b/lib/src/main/java/com/anyun/exam/lib/net/NtripTcpClient.kt
new file mode 100644
index 0000000..fbaa6b9
--- /dev/null
+++ b/lib/src/main/java/com/anyun/exam/lib/net/NtripTcpClient.kt
@@ -0,0 +1,26 @@
+package com.anyun.exam.lib.net
+
+import android.util.Base64
+import android.util.Log
+import java.nio.charset.StandardCharsets
+
+class NtripTcpClient(session: TcpSession) : NettyTcpClient(session) {
+ companion object {
+
+ }
+
+ fun sendNtripRequest(clientName: String, mountPoint: String,
+ username: String, password: String) {
+ val request = "GET /" + mountPoint + " HTTP/1.1\r\n" + "Host: " +
+ clientName + "\r\n" + "User-Agent: NTRIP " +
+ clientName + "\r\n" + "Authorization: Basic " +
+ encodeBase64(username + ":" + password) + "\r\n" +
+ "Ntrip-Version: Ntrip/2.0\r\n" + "Connection: keep-alive\r\n\r\n"
+
+ writeAndFlush(request)
+ }
+
+ private fun encodeBase64(value: String): String? {
+ return Base64.encodeToString(value.toByteArray(StandardCharsets.UTF_8), Base64.NO_WRAP)
+ }
+}
\ No newline at end of file
diff --git a/lib/src/main/java/com/anyun/exam/lib/net/ParseNtripPackage.kt b/lib/src/main/java/com/anyun/exam/lib/net/ParseNtripPackage.kt
new file mode 100644
index 0000000..1ba4002
--- /dev/null
+++ b/lib/src/main/java/com/anyun/exam/lib/net/ParseNtripPackage.kt
@@ -0,0 +1,147 @@
+package com.anyun.exam.lib.net
+
+import android.util.Log
+import com.anyun.exam.lib.ActionArgs
+import com.anyun.exam.lib.ActionManager
+import com.anyun.exam.lib.util.ByteUtil
+
+class ParseNtripPackage {
+ companion object {
+ val SYNC_HEAD = 0
+ val GET_LENGTH_HI = 1
+ val GET_LENGTH_LO = 2
+ val GET_PAYLOAD = 3
+ val GET_CRC_1 = 4
+ val GET_CRC_2 = 5
+ val GET_CRC_3 = 6
+ }
+
+ private val table = intArrayOf(
+ 0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17,
+ 0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E, 0xC54E89, 0x430272,
+ 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E, 0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646,
+ 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, 0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3,
+ 0x15E205, 0x93AEFE, 0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7,
+ 0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077, 0x681E59, 0xEE52A2,
+ 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E, 0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24,
+ 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5, 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1,
+ 0xA11107, 0x275DFC, 0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C,
+ 0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, 0x15723B, 0x933EC0,
+ 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, 0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4,
+ 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15, 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8,
+ 0xC90F5E, 0x4F43A5, 0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C,
+ 0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3, 0x92C69D, 0x148A66,
+ 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A, 0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB,
+ 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, 0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E,
+ 0x4EBBF8, 0xC8F703, 0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A,
+ 0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863, 0xFAD8C4, 0x7C943F,
+ 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3, 0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B,
+ 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA, 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C,
+ 0x33D79A, 0xB59B61, 0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58,
+ 0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, 0x4E2BC6, 0xC8673D,
+ 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1, 0x26359F, 0xA07964, 0xACE092, 0x2AAC69,
+ 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88, 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC,
+ 0x9E874A, 0x18CBB1, 0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401,
+ 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538)
+
+ private var rtcmPayloadLength = 0
+ private val rtcmPackage = ByteArray(1100)
+ private var parse_status = SYNC_HEAD
+ private var rx_len = 0
+ private var actionManager = ActionManager()
+
+ fun parse(tcpClient: NtripTcpClient, data: ByteArray) {
+ var x = 0
+ while (x < data.size) {
+ when (parse_status) {
+ SYNC_HEAD -> {
+ if ((data[x].toInt() and 0xFF) == 0xD3) {
+ parse_status = GET_LENGTH_HI
+ rtcmPackage[0] = data[x]
+ }
+ x++
+ }
+ GET_LENGTH_HI -> {
+ if ((data[x].toInt() and 0xFF) <= 0x03) {
+ parse_status = GET_LENGTH_LO
+ rtcmPackage[1] = data[x]
+ } else {
+ parse_status = SYNC_HEAD
+ }
+ x++
+ }
+ GET_LENGTH_LO -> {
+ parse_status = GET_PAYLOAD
+ rtcmPackage[2] = data[x]
+ rtcmPayloadLength = ((rtcmPackage[1].toInt() and 0xFF) shl 8) + (rtcmPackage[2].toInt() and 0xFF)
+ rx_len = 0
+ x++
+ }
+ GET_PAYLOAD -> {
+ if (data.size - x >= rtcmPayloadLength - rx_len) {
+ System.arraycopy(
+ data,
+ x,
+ rtcmPackage,
+ 3 + rx_len,
+ rtcmPayloadLength - rx_len
+ )
+ x += rtcmPayloadLength - rx_len
+ rx_len = rtcmPayloadLength
+ parse_status = GET_CRC_1
+ } else {
+ System.arraycopy(data, x, rtcmPackage, 3 + rx_len, data.size - x)
+ rx_len += data.size - x
+ x = data.size
+ }
+ }
+ GET_CRC_1 -> {
+ rtcmPackage[3 + rtcmPayloadLength] = data[x]
+ parse_status = GET_CRC_2
+ x++
+ }
+ GET_CRC_2 -> {
+ rtcmPackage[3 + rtcmPayloadLength + 1] = data[x]
+ parse_status = GET_CRC_3
+ x++
+ }
+ GET_CRC_3 -> {
+ rtcmPackage[3 + rtcmPayloadLength + 2] = data[x]
+ if (checkCrc()) {
+ val ba = ByteArray(rtcmPayloadLength + 6)
+ System.arraycopy(rtcmPackage, 0, ba, 0, rtcmPayloadLength + 6)
+ actionManager.execute(ActionArgs().apply {
+ this.message = ba
+ this.tcpClient = tcpClient
+ this.context = tcpClient.context
+ })
+ } else {
+ Log.d(Constant.LOGTAG, "${ByteUtil.byte2hex(rtcmPackage, 6+rtcmPayloadLength)}")
+ }
+ parse_status = SYNC_HEAD
+ x++
+ }
+ }
+ }
+ }
+
+ fun reset() {
+ parse_status = SYNC_HEAD
+ }
+
+ private fun checkCrc(): Boolean {
+ var crc = 0
+
+ for (i in 0 until 3 + rtcmPayloadLength) {
+ crc = crc shl 8 and 0xFFFFFF xor table[crc shr 16 xor (rtcmPackage[i].toInt() and 0xff)]
+ }
+
+ val res = (crc == ((rtcmPackage[3+rtcmPayloadLength].toInt() and 0xFF) shl 16) + ((rtcmPackage[3 + rtcmPayloadLength + 1].toInt() and 0xFF) shl 8) + (rtcmPackage[3 + rtcmPayloadLength + 2].toInt() and 0xFF))
+
+ if (!res) {
+ Log.d(Constant.LOGTAG, "鏍￠獙澶辫触 Length: ${rtcmPayloadLength} $crc e: ${((rtcmPackage[3+rtcmPayloadLength].toInt() and 0xFF) shl 16) + ((rtcmPackage[3 + rtcmPayloadLength + 1].toInt() and 0xFF) shl 8) + (rtcmPackage[3 + rtcmPayloadLength + 2].toInt() and 0xFF)}")
+ }
+
+ return res
+ }
+}
\ No newline at end of file
diff --git a/lib/src/main/java/com/anyun/exam/lib/net/TcpSession.kt b/lib/src/main/java/com/anyun/exam/lib/net/TcpSession.kt
new file mode 100644
index 0000000..a3e0688
--- /dev/null
+++ b/lib/src/main/java/com/anyun/exam/lib/net/TcpSession.kt
@@ -0,0 +1,13 @@
+package com.anyun.exam.lib.net
+
+class TcpSession
+{
+ companion object {
+ public val TRAIN_PLATFORM = 0
+ public val LOG_PLATFORM = 1
+ public val SEC_TRAIN_PLATFORM = 2
+ }
+ lateinit var ip:String
+ var port:Int = 0
+ lateinit var phone:String
+}
diff --git a/lib/src/main/java/com/anyun/exam/lib/util/ByteUtil.java b/lib/src/main/java/com/anyun/exam/lib/util/ByteUtil.java
index 0929b60..765cc92 100644
--- a/lib/src/main/java/com/anyun/exam/lib/util/ByteUtil.java
+++ b/lib/src/main/java/com/anyun/exam/lib/util/ByteUtil.java
@@ -13,4 +13,19 @@
}
return h.toString();
}
+
+ public static String byte2hex(byte [] buffer, int length) {
+ byte [] temp = new byte[length];
+ System.arraycopy(buffer, 0, temp, 0, length);
+ return byte2hex(temp);
+ }
+
+ public static boolean allIsAscii(byte []buffer) {
+ for (int i = 0; i < buffer.length; i++) {
+ if (buffer[i] < 0x20 || buffer[i] >= 0x7F) {
+ return false;
+ }
+ }
+ return true;
+ }
}
--
Gitblit v1.8.0