坐标

yy1717

2024-02-28 27fc91fbe8f88b6885356e68828cfe1ce1db7601

坐标

 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

 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);
    }


 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());
        // 向UI上报车辆点坐标
        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());
            // 向UI上报车辆点坐标
            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);
}


 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;             // 用RTK定位计算的行驶里程
} 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

 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

 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("路id %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:

 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);

 lib/src/main/cpp/mcu/car_box.cpp

New file
@@ -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);
}

 lib/src/main/cpp/mcu/car_box.h

New file
@@ -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

 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)

 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;
        }


 lib/src/main/cpp/rtcm.cpp

New file
@@ -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;
}

 lib/src/main/cpp/rtcm.h

New file
@@ -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

 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);
        }
    }

 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

 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");

 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;

 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:

 lib/src/main/cpp/teach/teach.cpp

New file
@@ -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());
    }
}

 lib/src/main/cpp/teach/teach.h

New file
@@ -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

 lib/src/main/cpp/teach/train.cpp

New file
@@ -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);
}

 lib/src/main/cpp/teach/train.h

New file
@@ -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

 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 线端和Y轴的夹角
 * p1----------->p2 线端和Y轴的夹角(顺时针)
 * @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;


 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);

 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;
    }

 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);


 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();
}

 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

 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;
}

 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

 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;
}

 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

 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;
}

 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

 lib/src/main/cpp/test_items/right_corner.cpp

New file
@@ -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;
}

 lib/src/main/cpp/test_items/right_corner.h

New file
@@ -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

 lib/src/main/cpp/test_items/stop_and_start.h

File was deleted

 lib/src/main/cpp/test_items/turn_a90.cpp

File was deleted

 lib/src/main/cpp/test_items/turn_a90.h

File was deleted

 lib/src/main/cpp/test_items/uphill.cpp

File was renamed from lib/src/main/cpp/test_items/stop_and_start.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);


 lib/src/main/cpp/test_items/uphill.h

New file
@@ -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

 lib/src/main/java/com/anyun/exam/lib/ActionManager.kt

New file
@@ -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
}

 lib/src/main/java/com/anyun/exam/lib/ActionRtcm.kt

New file
@@ -0,0 +1,7 @@
package com.anyun.exam.lib

class ActionRtcm: BaseAction {
    override fun execute(args: ActionArgs) {
        Rtcm.sendRtcm(args.message)
    }
}

 lib/src/main/java/com/anyun/exam/lib/BaseAction.kt

New file
@@ -0,0 +1,5 @@
package com.anyun.exam.lib

interface BaseAction {
    fun execute(args:ActionArgs)
}

 lib/src/main/java/com/anyun/exam/lib/IRtcmData.kt

New file
@@ -0,0 +1,5 @@
package com.anyun.exam.lib

interface IRtcmData {
    fun sendRtcm(data: ByteArray, length: Int)
}

 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);
}

 lib/src/main/java/com/anyun/exam/lib/Rtcm.kt

New file
@@ -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)
    }
}

 lib/src/main/java/com/anyun/exam/lib/net/ConnectionWatchdog.kt

New file
@@ -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}] 需要手动重新连接")
        }
    }
}


 lib/src/main/java/com/anyun/exam/lib/net/ConnectorIdleStateTrigger.kt

New file
@@ -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)
        }
    }
}


 lib/src/main/java/com/anyun/exam/lib/net/Constant.kt

New file
@@ -0,0 +1,8 @@
package com.anyun.exam.lib.net

class Constant {
    companion object
    {
        val LOGTAG = "ntrip"
    }
}

 lib/src/main/java/com/anyun/exam/lib/net/ExceptionHandler.kt

New file
@@ -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())
//            }
//        })
//    }

}

 lib/src/main/java/com/anyun/exam/lib/net/MessageHandler.kt

New file
@@ -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, "开始接收RTCM数据")
                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
//        })
//
//    }

}

 lib/src/main/java/com/anyun/exam/lib/net/NettyTcpClient.kt

New file
@@ -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>() {
            //初始化channel
            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
    }
}

 lib/src/main/java/com/anyun/exam/lib/net/NtripDecoder.kt

New file
@@ -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)
    }
}

 lib/src/main/java/com/anyun/exam/lib/net/NtripEncoder.kt

New file
@@ -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))
        }
    }
}

 lib/src/main/java/com/anyun/exam/lib/net/NtripTcpClient.kt

New file
@@ -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)
    }
}

 lib/src/main/java/com/anyun/exam/lib/net/ParseNtripPackage.kt

New file
@@ -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
    }
}

 lib/src/main/java/com/anyun/exam/lib/net/TcpSession.kt

New file
@@ -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
}

 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;
    }
}