坐标

yy1717

2024-02-28 27fc91fbe8f88b6885356e68828cfe1ce1db7601

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