坐标

yy1717

2024-02-28 27fc91fbe8f88b6885356e68828cfe1ce1db7601

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