//
|
// Created by YY on 2020/3/24.
|
//
|
|
#include "stop_car.h"
|
#include "../driver_test.h"
|
#include "../native-lib.h"
|
#include "../jni_log.h"
|
#include "road_exam.h"
|
#include "../utils/xconvert.h"
|
#include "../common/apptimer.h"
|
#include "../test_common/car_sensor.h"
|
#include "../defs.h"
|
#include "../test_common/odo_graph.h"
|
|
#define DEBUG(fmt, args...) LOGD("<road_exam stop_car> <%s>: " fmt, __func__, ##args)
|
|
static bool BreakHandbreakReleaseSametime, OpenDoor;
|
|
static const int ENGINE_MIN_ROTATE = 100;
|
|
static double beginOdo;
|
static int setup;
|
|
static void TtsBack(int seq)
|
{
|
setup = 1;
|
}
|
|
void StartStopCarExam(void) {
|
DEBUG("靠边停车");
|
BreakHandbreakReleaseSametime = false;
|
setup = 0;
|
OpenDoor = false;
|
PlayTTS(examParam.stop_car_begin_tts, TtsBack);
|
}
|
|
bool TestStopCar(road_exam_map &RoadMap, int roadIndex, const car_model *car, int moveDirect, const struct RtkTime *rtkTime) {
|
static struct RtkTime time;
|
|
if (setup == 0)
|
return true;
|
|
if (roadIndex < 0) {
|
DEBUG("停车距离超标，靠边停车结束");
|
AddExamFault(33, rtkTime);
|
return false;
|
}
|
|
if (setup == 1) {
|
beginOdo = ReadOdo();
|
setup = 2;
|
} else if (setup == 2) {
|
if (moveDirect == 0) {
|
time = *rtkTime;
|
setup = 3;
|
}
|
} else if (setup == 3) {
|
if (moveDirect != 0) {
|
setup = 2;
|
} else if (TimeGetDiff(rtkTime, &time) >= D_SEC(2)) {
|
// 停车超2秒，开始判断
|
DEBUG("检测和路边的距离");
|
|
PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ]);
|
PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ]);
|
|
if (DistanceOf(p1, car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ]) > examParam.stop_car_edge_red_distance ||
|
DistanceOf(p2, car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ]) > examParam.stop_car_edge_red_distance) {
|
DEBUG("停车超出路边0.5米");
|
// 停车距离超过50厘米，不合格
|
AddExamFault(36, rtkTime);
|
return false;
|
} else if (DistanceOf(p1, car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ]) > examParam.stop_car_edge_yellow_distance ||
|
DistanceOf(p2, car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ]) > examParam.stop_car_edge_yellow_distance) {
|
DEBUG("停车超出路边0.3米");
|
// 停车距离超过30厘米，扣10分
|
AddExamFault(37, rtkTime);
|
}
|
|
setup = 4;
|
}
|
} else if (setup == 4) {
|
car_sensor_value_t brk = ReadCarSensorValue(BREAK);
|
car_sensor_value_t hbrk = ReadCarSensorValue(HAND_BREAK);
|
car_sensor_value_t door = ReadCarSensorValue(DOOR);
|
car_sensor_value_t rpm = ReadCarSensorValue(ENGINE_RPM);
|
|
if (!BreakHandbreakReleaseSametime && brk.value == BREAK_INACTIVE && hbrk.value == BREAK_INACTIVE) {
|
// 拉手刹前，松脚刹，扣10分
|
DEBUG("拉手刹前，松脚刹");
|
AddExamFault(39, rtkTime);
|
BreakHandbreakReleaseSametime = true;
|
}
|
|
if (door.value == DOOR_OPEN) {
|
if (!OpenDoor) {
|
if (rpm.value > ENGINE_MIN_ROTATE) {
|
// 下车前，不熄火，扣5分
|
DEBUG("下车前，不熄火");
|
AddExamFault(40, rtkTime);
|
}
|
if (hbrk.value == BREAK_INACTIVE) {
|
// 开门前，未拉手刹， 扣10分
|
DEBUG("开门前，未拉手刹");
|
AddExamFault(38, rtkTime);
|
}
|
time = *rtkTime;
|
OpenDoor = true;
|
}
|
|
if (TimeGetDiff(rtkTime, &time) > examParam.stop_car_open_door_allow_time) {
|
// 开门时间超过15秒，不合格
|
DEBUG("开门时间超过15秒");
|
AddExamFault(35, rtkTime);
|
return false;
|
}
|
}
|
|
if (OpenDoor && door.value == DOOR_CLOSE) {
|
DEBUG("完成停车");
|
PlayTTS(examParam.stop_car_end_tts, NULL);
|
return false;
|
}
|
}
|
|
if (ReadOdo() - beginOdo > examParam.stop_car_limit_distance) {
|
// 150米内未停车，不合格
|
DEBUG("停车距离超标，靠边停车结束");
|
AddExamFault(33, rtkTime);
|
return false;
|
}
|
|
return true;
|
}
|
