//
|
// Created by YY on 2019/10/21.
|
// Units note: distance - metre
|
// speed - metre per second
|
// angle - DEGREES
|
//
|
|
#include <cstdlib>
|
#include <cmath>
|
#include <semaphore.h>
|
#include <pthread.h>
|
#include <cstring>
|
#include <vector>
|
#include <list>
|
#include <numeric>
|
#include <algorithm>
|
#include <string>
|
|
#include "driver_test.h"
|
#include "defs.h"
|
#include "test_common/Geometry.h"
|
#include "common/apptimer.h"
|
#include "jni_log.h"
|
#include "test_items/park_edge.h"
|
#include "native-lib.h"
|
#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/driving_curve.h"
|
#include "test_items/stop_and_start.h"
|
#include "master/comm_if.h"
|
#include "utils/xconvert.h"
|
#include "utils/num.h"
|
#include "test_common/car_sensor.h"
|
#include "mcu/mcu_if.h"
|
#include "test_common/car_sensor.h"
|
#include "test_items2/road_exam.h"
|
#include "test_items/area_exam.h"
|
#include "test_items2/prepare.h"
|
#include "test_common/odo_graph.h"
|
|
#define DEBUG(fmt, args...) LOGD("<driver_test> <%s>: " fmt, __func__, ##args)
|
|
using namespace std;
|
|
#define RTK_INVALID 0
|
#define RTK_SINGLE_POINT 1
|
#define RTK_DIFF 2
|
#define RTK_FIX 3
|
#define RTK_FLOAT 4
|
|
#define CAR_MODEL_POINT_NUM 32
|
#define MAP_LIST_SIZE 32
|
|
enum {
|
TEST_TYPE_AREA = 2,
|
TEST_TYPE_ROAD_DUMMY_LIGHT,
|
TEST_TYPE_ROAD_TRUE_LIGHT,
|
TEST_TYPE_ROAD_CALIBRATE
|
};
|
|
static const int DEFAULT_START_KEY_HOLD_TIME = D_SEC(2);
|
static const int DEFAULT_CURVE_PAUSE_TIME = D_SEC(2);
|
static const int DEFAULT_PARK_BOTTOM_PAUSE_TIME = D_SEC(2);
|
static const int DEFAULT_PARK_BOTTOM_FINISH_TIME = D_SEC(210);
|
static const int DEFAULT_PART_EDGE_PAUSE_TIME = D_SEC(2);
|
static const int DEFAULT_PARK_EDGE_FINISH_TIME = D_SEC(90);
|
static const int DEFAULT_TURN_A90_PAUSE_TIME = D_SEC(2);
|
static const int DEFAULT_RAMP_FINISH_TIME = D_SEC(30);
|
static const double DEFAULT_RAMP_STOPPOINT_RED_DISTANCE = 0.5;
|
static const double DEFAULT_RAMP_EDGE_YELLOW_DISTANCE = 0.3;
|
static const double DEFAULT_RAMP_EDGE_RED_DISTANCE = 0.5;
|
static const double DEFAULT_RAMP_SLIDE_YELLOW_DISTANCE = 0.1;
|
static const double DEFAULT_RAMP_SLIDE_RED_DISTANCE = 0.3;
|
|
static const double DEFAULT_ROAD_SLIDE_YELLOW_DISTANCE = 0.1;
|
static const double DEFAULT_ROAD_SLIDE_RED_DISTANCE = 0.3;
|
static const int DEFAULT_ROAD_MAX_DISTANCE = 3000;
|
static const int DEFAULT_ROAD_MAX_SPEED = 60;
|
static const int DEFAULT_GEAR_N_TIME = D_SEC(5);
|
static const int DEFAULT_SAME_GEAR_HOLD_TIME = D_SEC(5);
|
static const int DEFAULT_GEAR_SPEED_ERROR_MAX_TIME = D_SEC(15);
|
static const int DEFAULT_ROAD_PAUSE_TIME = D_SEC(2);
|
static const int DEFAULT_CHANGE_LANE_MIN_TIME = D_SEC(10);
|
static const int DEFAULT_CRASH_DOTTED_LINE_MAX_TIME = D_SEC(10);
|
static const int DEFAULT_TURN_SIGNAL_MIN_ADVANCE = D_SEC(3);
|
static const int DEFAULT_START_CAR_MAX_RMP = 2500;
|
static const int DEFAULT_START_CAR_DISTANCE = 10;
|
static const double DEFAULT_START_CAR_OPEN_DOOR_DISTANCE = 1.0;
|
static const char DEFAULT_PREPARE_TTS[] = "请开始上车准备";
|
static const char DEFAULT_TOUCH_LEFT_FRONT[] = "学员通过左前方";
|
static const char DEFAULT_TOUCH_LEFT_REAR[] = "学员通过左后方";
|
static const char DEFAULT_TOUCH_RIGHT_FRONT[] = "学员通过右前方";
|
static const char DEFAULT_TOUCH_RIGHT_REAR[] = "学员通过右后方";
|
static const char DEFAULT_START_ENGINE[] = "请启动发动机";
|
static const char DEFAULT_START_CAR_BEGIN_TTS[] = "请起步，继续完成考试";
|
static const char DEFAULT_START_CAR_END_TTS[] = "起步完成";
|
static const int CHANGE_LANE_MAX_DISTANCE = 100;
|
static const char DEFAULT_CHANGE_LANE_BEGIN_TTS[] = "前方请变更车道";
|
static const char DEFAULT_CHANGE_LANE_END_TTS[] = "变道完成";
|
static const int DEFAULT_SHIFT_MAX_DISTANCE = 120;
|
static const int DEFAULT_SHIFT_HOLD_TIME = D_SEC(3);
|
static const char DEFAULT_SHIFT_BEGIN_TTS[] = "请进行加减挡位操作";
|
static const char DEFAULT_SHIFT_END_TTS[] = "加减挡位完成";
|
static const char DEFAULT_STRAIGHT_BEGIN_TTS[] = "请保持直线行驶";
|
static const char DEFAULT_STRAIGHT_END_TTS[] = "直线行驶完成";
|
static const int DEFAULT_STRAIGHT_MAX_DISTANCE = 100;
|
static const double DEFAULT_STRAIGHT_MAX_OFFSET = 0.3;
|
static const int DEFAULT_OVERTAKE_MAX_DISTANCE = 150;
|
static const char DEFAULT_OVERTAKE_BEGIN_TTS[] = "请超越前方车辆";
|
static const char DEFAULT_OVERTAKE_END_TTS[] = "超车完成";
|
static const int DEFAULT_STOP_CAR_MAX_DISTANCE = 150;
|
static const int DEFAULT_STOP_CAR_OPEN_DOOR_MAX_TIME = D_SEC(15);
|
static const double DEFAULT_STOP_CAR_EDGE_RED_DISTANCE = 0.5;
|
static const double DEFAULT_STOP_CAR_EDGE_YELLOW_DISTANCE = 0.5;
|
static const char DEFAULT_STOP_CAR_BEGIN_TTS[] = "请靠边停车";
|
static const char DEFAULT_STOP_CAR_END_TTS[] = "靠边停车完成";
|
static const double DEFAULT_CROSSING_STOP_VALID_DISTANCE = 3.0;
|
static const int DEFAULT_CROSS_SCHOOL_MAX_SPEED = 30;
|
static const int DEFAULT_CROSS_BREAK_VALID_DISTANCE = 30;
|
|
static const char DEFAULT_CROSSING_GO_STRAIGHT_TTS[] = "前方路口直行";
|
static const char DEFAULT_CROSSING_TURN_LEFT_TTS[] = "前方路口左转";
|
static const char DEFAULT_CROSSING_TURN_RIGHT_TTS[] = "前方路口右转";
|
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 road_exam_map RoadMap;
|
|
static int exam_dummy_light;
|
|
static car_model *CarModel = NULL;
|
|
static LIST_CAR_MODEL CarModelList; // 一段时间的车辆轨迹集合
|
|
static struct dummy_light_exam *DummyLightContent;
|
static int DummyLightContentSize;
|
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
|
#define CAR_MODEL_CACHE_SIZE 10
|
|
static rtk_info *RtkBuffer = NULL;
|
static int RtkBufferNum = 0, RtkBufferIn = 0;
|
|
static pthread_mutex_t clock_mutex = PTHREAD_MUTEX_INITIALIZER;
|
static struct RtkTime rtkClock;
|
|
static void SetExamParamDefault(void);
|
static void EngineStartHold(union sigval sig);
|
static void ExecuteExam(const struct RtkTime* rtkTime);
|
static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime);
|
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b);
|
static void ReadDriverExamPrimerTimeout(union sigval sig);
|
static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel);
|
static bool UpdateCarCoord(double &spd, int &mov, int &idx);
|
|
static void PrintObdInfo(struct RtkTime *rtkTime, double speed);
|
|
void DriverTestInit(void)
|
{
|
ExamStart = false;
|
SetExamParamDefault();
|
|
CarModel = NULL;
|
CarModelList.clear();
|
|
AreaMapList.clear();
|
|
RoadMap.roads.clear();
|
RoadMap.specialAreas.clear();
|
RoadMap.forbidLines.clear();
|
RoadMap.examScheme.clear();
|
|
CarSensorInit();
|
|
DummyLightContentSize = 0;
|
DummyLightContent = NULL;
|
|
RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
|
RtkBufferNum = RtkBufferIn = 0;
|
|
pthread_mutex_init(&clock_mutex, NULL);
|
}
|
|
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;
|
|
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;
|
|
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;
|
|
examParam.gear_speed_table.resize(6);
|
for (int i = 0; i < examParam.gear_speed_table.size(); ++i) {
|
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;
|
|
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;
|
|
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;
|
|
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 = "";
|
|
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;
|
|
examParam.overtake_limit_distance = DEFAULT_OVERTAKE_MAX_DISTANCE;
|
examParam.overtake_begin_tts = DEFAULT_OVERTAKE_BEGIN_TTS;
|
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;
|
|
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;
|
}
|
|
static void ReadDriverExamPrimerTimeout(union sigval sig)
|
{
|
AppTimer_delete(ReadDriverExamPrimerTimeout);
|
AppTimer_add(ReadDriverExamPrimerTimeout, D_SEC(2));
|
ReadDriverExamPrimer();
|
}
|
|
void ReadDriverExamPrimer(void)
|
{
|
MA_ReadMap();
|
MA_ReadCar();
|
MA_ReadSensor();
|
}
|
|
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);
|
}
|
|
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);
|
}
|
|
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());
|
}
|
|
void SetRoadExamScheme(vector<scheme_t> &scheme)
|
{
|
if (ExamStart) return;
|
|
vector<scheme_t>().swap(RoadMap.examScheme);
|
|
RoadMap.examScheme.assign(scheme.begin(), scheme.end());
|
|
DEBUG("得到路考项目方案 项目数量 %d", RoadMap.examScheme.size());
|
}
|
|
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
|
int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
|
int *body, int bodyNum, double (*point)[2], int pointNum, double antPitch, double antHeight, double groundHeight)
|
{
|
DEBUG("加入车辆信息 pointNum %d", pointNum);
|
|
if (point == NULL || pointNum == 0 || ExamStart) return;
|
|
if (CarModel != NULL) {
|
if (CarModel->body != NULL)
|
free(CarModel->body);
|
if (CarModel->carDesc != NULL)
|
free(CarModel->carDesc);
|
if (CarModel->carXY != NULL)
|
free(CarModel->carXY);
|
free(CarModel);
|
CarModel = NULL;
|
}
|
|
CarModel = (car_model *)malloc(sizeof(car_model));
|
CarModel->basePoint.X = basePoint[0];
|
CarModel->basePoint.Y = basePoint[1];
|
CarModel->axial[0] = axial[0];
|
CarModel->axial[1] = axial[1];
|
CarModel->left_front_tire[0] = left_front_tire[0];
|
CarModel->left_front_tire[1] = left_front_tire[1];
|
CarModel->right_front_tire[0] = right_front_tire[0];
|
CarModel->right_front_tire[1] = right_front_tire[1];
|
CarModel->left_rear_tire[0] = left_rear_tire[0];
|
CarModel->left_rear_tire[1] = left_rear_tire[1];
|
CarModel->right_rear_tire[0] = right_rear_tire[0];
|
CarModel->right_rear_tire[1] = right_rear_tire[1];
|
|
CarModel->bodyNum = bodyNum;
|
if (bodyNum == 0 || body == NULL) {
|
CarModel->bodyNum = 6;
|
CarModel->body = (int *) malloc(sizeof(int) * 6);
|
for (int i = 0; i < 6; ++i) {
|
CarModel->body[i] = i;
|
}
|
} else {
|
CarModel->body = (int *) malloc(sizeof(int) * CarModel->bodyNum);
|
for (int i = 0; i < CarModel->bodyNum; ++i) {
|
CarModel->body[i] = body[i];
|
}
|
}
|
|
CarModel->antPitch = antPitch;
|
CarModel->antHeight = antHeight;
|
CarModel->groundHeight = groundHeight;
|
|
CarModel->pointNum = pointNum;
|
CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * pointNum);
|
CarModel->carXY = (PointF *) malloc(sizeof(PointF) * pointNum);
|
|
// 测量坐标转换为距离-角度形式
|
double C02 = (point[0][0]-basePoint[0])*(point[0][0]-basePoint[0]) +
|
(point[0][1]-basePoint[1])*(point[0][1]-basePoint[1]);
|
double C0 = sqrt(C02);
|
|
CarModel->carDesc[0].distance = sqrt(C02);
|
CarModel->carDesc[0].angle = 0.0;
|
|
for (int i = 1; i < pointNum; ++i) {
|
double dis2 = (point[i][0]-basePoint[0])*(point[i][0]-basePoint[0]) +
|
(point[i][1]-basePoint[1])*(point[i][1]-basePoint[1]);
|
double dis = sqrt(dis2);
|
|
CarModel->carDesc[i].distance = dis;
|
|
CarModel->carDesc[i].angle = 180 * acos((dis2 + C02 - ((point[i][0]-point[0][0])*(point[i][0]-point[0][0]) +
|
(point[i][1]-point[0][1])*(point[i][1]-point[0][1])))/(2*C0*dis)) / M_PI;
|
|
if (i > axial[1])
|
CarModel->carDesc[i].angle = 360.0 - CarModel->carDesc[i].angle;
|
|
DEBUG("加入点<%d> 距离 %f 角度 %f", i, CarModel->carDesc[i].distance, CarModel->carDesc[i].angle);
|
}
|
|
// CarModel->carDesc[0].distance = 0.2465;
|
// CarModel->carDesc[0].angle = 0;
|
//
|
// CarModel->carDesc[1].distance = 0.2635;
|
// CarModel->carDesc[1].angle = 20.7;
|
//
|
// CarModel->carDesc[2].distance = 0.14;
|
// CarModel->carDesc[2].angle = 138.9;
|
//
|
// CarModel->carDesc[3].distance = 0.1055;
|
// CarModel->carDesc[3].angle = 180.0;
|
//
|
// CarModel->carDesc[4].distance = 0.14;
|
// CarModel->carDesc[4].angle = 221.1;
|
//
|
// CarModel->carDesc[5].distance = 0.2635;
|
// CarModel->carDesc[5].angle = 339.3;
|
|
DEBUG("SetCarMeasurePoint Calc Over");
|
}
|
|
/***********************************************
|
* TIME1 - TIME2: millisecond
|
* @param rtkTime1
|
* @param rtkTime2
|
* @return
|
*/
|
uint32_t TimeGetDiff(const struct RtkTime *rtkTime1, const struct RtkTime *rtkTime2)
|
{
|
char tm1[64], tm2[64];
|
|
sprintf(tm1, "%02d%02d%02d%02d%02d%02d%03d", rtkTime1->YY, rtkTime1->MM, rtkTime1->DD, rtkTime1->hh, rtkTime1->mm, rtkTime1->ss, rtkTime1->mss*10);
|
sprintf(tm2, "%02d%02d%02d%02d%02d%02d%03d", rtkTime2->YY, rtkTime2->MM, rtkTime2->DD, rtkTime2->hh, rtkTime2->mm, rtkTime2->ss, rtkTime2->mss*10);
|
|
if (strcmp(tm1, tm2) < 0) {
|
return (uint32_t)(-1);
|
}
|
|
if (rtkTime1->YY == rtkTime2->YY && rtkTime1->MM == rtkTime2->MM && rtkTime1->DD == rtkTime2->DD) {
|
return TimeGetDiff(rtkTime1->hh, rtkTime1->mm, rtkTime1->ss, rtkTime1->mss*10,
|
rtkTime2->hh, rtkTime2->mm, rtkTime2->ss, rtkTime2->mss*10);
|
} else {
|
return (TimeMakeComposite(2000 + rtkTime1->YY, rtkTime1->MM, rtkTime1->DD, rtkTime1->hh, rtkTime1->mm, rtkTime1->ss) -
|
TimeMakeComposite(2000 + rtkTime2->YY, rtkTime2->MM, rtkTime2->DD, rtkTime2->hh, rtkTime2->mm, rtkTime2->ss)) * 1000
|
+ (1000 + rtkTime1->mss*10 - rtkTime2->mss*10) % 1000;
|
}
|
}
|
|
void SetDummyLightExam(int n, struct dummy_light_exam *cfg)
|
{
|
DEBUG("获取模拟路考灯光测试项目 N = %d %d", n, ExamStart);
|
|
static const int CONV_TABLE[] = {(FLASH_BEAM_LAMP<<8)+OFF_LIGHT,
|
(TURN_SIGNAL_LAMP<<8)+OFF_LIGHT,
|
(TURN_SIGNAL_LAMP<<8)+OFF_LIGHT,
|
(TURN_SIGNAL_LAMP<<8)+OFF_LIGHT,
|
(FOG_LAMP<<8)+OFF_LIGHT,
|
(CLEARANCE_LAMP<<8)+OFF_LIGHT,
|
(MAIN_BEAM_LAMP<<8)+OFF_LIGHT,
|
(DIPPED_BEAM_LAMP<<8)+OFF_LIGHT,
|
0,
|
(DIPPED_BEAM_LAMP<<8)+DIPPED_BEAM_LIGHT,
|
(MAIN_BEAM_LAMP<<8)+MAIN_BEAM_LIGHT,
|
(CLEARANCE_LAMP<<8)+CLEARANCE_LIGHT,
|
(FOG_LAMP<<8)+FOG_LIGHT,
|
(TURN_SIGNAL_LAMP<<8)+LEFT_TURN_LIGHT,
|
(TURN_SIGNAL_LAMP<<8)+RIGHT_TURN_LIGHT,
|
(TURN_SIGNAL_LAMP<<8)+HAZARD_LIGHTS,
|
(FLASH_BEAM_LAMP<<8)+FLASH_BEAM_LIGHT};
|
|
const int *cov = CONV_TABLE + 8;
|
|
if (ExamStart) return;
|
|
if (DummyLightContent != NULL) {
|
delete []DummyLightContent;
|
DummyLightContent = NULL;
|
DummyLightContentSize = 0;
|
}
|
|
DummyLightContent = new struct dummy_light_exam[n];
|
DummyLightContentSize = n;
|
|
for (int i = 0; i < n; i++) {
|
DummyLightContent[i].item = cfg[i].item;
|
DummyLightContent[i].tts = cfg[i].tts;
|
DummyLightContent[i].wrongCode = cfg[i].wrongCode;
|
// Sensor Name<<8 + Sensor Status
|
for (int j = 0; j < cfg[i].process.size(); ++j) {
|
DummyLightContent[i].process.push_back(cov[cfg[i].process[j]]);
|
}
|
for (int j = 0; j < cfg[i].solution.size(); ++j) {
|
DummyLightContent[i].solution.push_back(cov[cfg[i].solution[j]]);
|
}
|
}
|
}
|
|
void StartDriverExam(int start, int type)
|
{
|
bool err = false;
|
DEBUG("++++++++++++考试启动 start %d type %d+++++++++++++", start, type);
|
|
if (start == 0) {
|
DEBUG("结束考试");
|
|
TerminateRoadExam();
|
TerminateAreaExam();
|
|
|
ExamStart = false;
|
MA_SendExamStatus(0, 0);
|
return;
|
}
|
|
// type = TEST_TYPE_ROAD_CALIBRATE;
|
|
if (AreaMapList.size() == 0 && type == TEST_TYPE_AREA) {
|
DEBUG("没有场考地图");
|
err = true;
|
MA_SendExamStatus(0, -1);
|
}
|
if (CarModel == NULL) {
|
DEBUG("没有车模");
|
err = true;
|
MA_SendExamStatus(0, -2);
|
}
|
if (DummyLightContent == NULL && type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
|
DEBUG("没有模拟灯光");
|
// 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) {
|
DEBUG("启动考试");
|
ExamFaultList.clear();
|
examFaultIndex = 0;
|
|
ExamStart = 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;
|
InitRoadExam(RoadMap);
|
}
|
if (type == TEST_TYPE_AREA) {
|
InitAreaExam();
|
}
|
if (type == TEST_TYPE_ROAD_CALIBRATE) {
|
RoadMap.calibrate = 1;
|
InitRoadExam(RoadMap);
|
}
|
}
|
MA_SendExamStatus(1, 0);
|
}
|
}
|
|
void UpdateRTKInfo(const rtk_info *s)
|
{
|
pthread_mutex_lock(&clock_mutex);
|
rtkClock.YY = s->YY;
|
rtkClock.MM = s->MM;
|
rtkClock.DD = s->DD;
|
rtkClock.hh = s->hh;
|
rtkClock.mm = s->mm;
|
rtkClock.ss = s->ss;
|
rtkClock.mss = s->dss;
|
pthread_mutex_unlock(&clock_mutex);
|
|
if (ExamStart) {
|
ExecuteExam(&rtkClock); // 执行无需车辆定位的项目
|
}
|
|
if (s->qf == 3) {
|
RtkBuffer[RtkBufferIn] = *s;
|
RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE;
|
if (RtkBufferNum < RTK_BUFFER_SIZE)
|
RtkBufferNum++;
|
} else {
|
return;
|
}
|
|
double speed;
|
int move;
|
int index;
|
|
if (UpdateCarCoord(speed, move, index)) {
|
struct carBrief brief;
|
|
sprintf(brief.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + RtkBuffer[index].YY,
|
RtkBuffer[index].MM, RtkBuffer[index].DD, RtkBuffer[index].hh,
|
RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);
|
|
brief.qf = RtkBuffer[index].qf;
|
brief.map_id = -1;//GetMapId(CurrExamMapIndex, MapList, MapNum);
|
brief.move = move;
|
brief.speed = speed * 3.6;
|
brief.heading = RtkBuffer[index].heading;
|
brief.main_ant[0] = RtkBuffer[index].x;
|
brief.main_ant[1] = RtkBuffer[index].y;
|
|
brief.axial[0] = CarModel->axial[0];
|
brief.axial[1] = CarModel->axial[1];
|
brief.left_front_tire[0] = CarModel->left_front_tire[0];
|
brief.left_front_tire[1] = CarModel->left_front_tire[1];
|
brief.right_front_tire[0] = CarModel->right_front_tire[0];
|
brief.right_front_tire[1] = CarModel->right_front_tire[1];
|
brief.left_rear_tire[0] = CarModel->left_rear_tire[0];
|
brief.left_rear_tire[1] = CarModel->left_rear_tire[1];
|
brief.right_rear_tire[0] = CarModel->right_rear_tire[0];
|
brief.right_rear_tire[1] = CarModel->right_rear_tire[1];
|
|
brief.bodyNum = CarModel->bodyNum;
|
brief.body = (int *) malloc(sizeof(int) * CarModel->bodyNum);
|
for (int i = 0; i < CarModel->bodyNum; ++i) {
|
brief.body[i] = CarModel->body[i];
|
}
|
|
brief.pointNum = CarModel->pointNum;
|
brief.point = (double *) malloc(CarModel->pointNum * 2 * sizeof(double));
|
for (int i = 0, j = 0; i < CarModel->pointNum; ++i) {
|
brief.point[j++] = round(CarModel->carXY[i].X, 4);
|
brief.point[j++] = round(CarModel->carXY[i].Y, 4);
|
}
|
|
MA_SendCarPosition(&brief);
|
|
free(brief.body);
|
free(brief.point);
|
|
struct RtkTime rtkTime;
|
double azimuth = RtkBuffer[index].heading;
|
|
rtkTime.YY = RtkBuffer[index].YY;
|
rtkTime.MM = RtkBuffer[index].MM;
|
rtkTime.DD = RtkBuffer[index].DD;
|
rtkTime.hh = RtkBuffer[index].hh;
|
rtkTime.mm = RtkBuffer[index].mm;
|
rtkTime.ss = RtkBuffer[index].ss;
|
rtkTime.mss = RtkBuffer[index].dss;
|
|
if (ExamStart) {
|
ExecuteExam(speed, move, azimuth, &rtkTime);
|
}
|
|
// PrintObdInfo(&rtkTime, speed);
|
}
|
}
|
|
static void PrintObdInfo(struct RtkTime *rtkTime, double speed) {
|
static struct RtkTime cTime = *rtkTime;
|
|
if (TimeGetDiff(rtkTime, &cTime) >= D_SEC(3)) {
|
cTime = *rtkTime;
|
DEBUG("GEAR %d RPM %d OBD_SPEED %f SPEED %f",
|
ReadCarStatus(GEAR) - GEAR_N,
|
ReadCarStatus(ENGINE_RPM),
|
((double)ReadCarStatus(OBD_SPEED)) / 10.0,
|
speed * 3.6);
|
}
|
}
|
|
static void ExecuteExam(const struct RtkTime* rtkTime)
|
{
|
{
|
static const char *NAME[] = {"OBD_SPEED",
|
"ENGINE_RPM",
|
"挡位",
|
"转向灯",
|
"近光灯",
|
"雾灯",
|
"示廓灯",
|
"闪灯提示",
|
"远光灯",
|
"安全带",
|
"启动引擎",
|
"刹车",
|
"手刹",
|
"副刹车",
|
"车门",
|
"绕车一",
|
"绕车二",
|
"绕车三",
|
"绕车四",
|
"CAR_STATUS_END"};
|
|
static const char *VALUE[] = {
|
"关闭",
|
"告警灯",
|
"左转信号",
|
"右转信号",
|
"示廓灯亮",
|
"近光灯亮",
|
"远光灯亮",
|
"远近切换",
|
"雾灯亮",
|
"插入",
|
"在启动位",
|
"空档",
|
"一档",
|
"二档",
|
"三档",
|
"四档",
|
"五档",
|
"倒挡",
|
"踩下",
|
"门关闭",
|
"绕车发生"
|
};
|
|
|
|
static int cs[CAR_STATUS_END] = {0};
|
|
int cs_temp[CAR_STATUS_END];
|
|
for (int i = 0; i < CAR_STATUS_END; ++i) {
|
// DEBUG("读取......");
|
cs_temp[i] = ReadCarStatus(i);
|
|
// DEBUG("读取 %d <---- %d", i, cs_temp[i]);
|
}
|
|
for (int i = 0; i < 2; ++i) {
|
if (cs_temp[i] != cs[i]) {
|
// DEBUG("车辆状态 %s = %d", NAME[i], cs_temp[i]);
|
|
cs[i] = cs_temp[i];
|
}
|
}
|
|
for (int i = 2; i < CAR_STATUS_END; ++i) {
|
if (cs_temp[i] != cs[i]) {
|
DEBUG("车辆状态 %s = %s", NAME[i], VALUE[ cs_temp[i] ]);
|
|
cs[i] = cs_temp[i];
|
|
// char buff[128];
|
// sprintf(buff, "%s，%s", NAME[i], VALUE[ cs_temp[i] ]);
|
// PlayTTS(buff, NULL);
|
}
|
}
|
|
}
|
|
if (ReadCarStatus(ENGINE_RPM) < ENGINE_MIN_ROTATE) {
|
if (engineRuning) {
|
engineRuning = false;
|
if (ExamType == TEST_TYPE_AREA) {
|
// 熄火1次，扣10分
|
AddExamFault(10210, rtkTime);
|
} else {
|
AddExamFault(30208, rtkTime);
|
}
|
}
|
} else {
|
engineRuning = true;
|
}
|
|
if (ReadCarStatus(ENGINE_START) == ENGINE_START_ACTIVE) {
|
if (!engineStart) {
|
DEBUG("检测到点火");
|
engineStart = true;
|
if (ReadCarStatus(GEAR) != GEAR_N) {
|
DEBUG("不在空挡点火");
|
// 不是空挡点火，不合格
|
if (ExamType == TEST_TYPE_AREA)
|
AddExamFault(10105, rtkTime);
|
else
|
AddExamFault(30105, rtkTime);
|
}
|
AppTimer_delete(EngineStartHold);
|
AppTimer_add(EngineStartHold, examParam.hold_start_key_limit_time);
|
}
|
} else if (engineStart) {
|
DEBUG("检测到关闭点火");
|
engineStart = false;
|
AppTimer_delete(EngineStartHold);
|
}
|
|
if (ExamType == TEST_TYPE_ROAD_DUMMY_LIGHT) {
|
if (exam_dummy_light == 0) {
|
StartPrepare();
|
// StartDummyLightExam(DummyLightContent, DummyLightContentSize, rtkTime);
|
exam_dummy_light = 1;
|
DEBUG("开始上车准备");
|
} else if (exam_dummy_light == 2) {
|
DEBUG("开始灯光考试");
|
StartDummyLightExam(DummyLightContent, DummyLightContentSize, rtkTime);
|
exam_dummy_light = 3;
|
} else if (exam_dummy_light == 3) {
|
if (!ExecuteDummyLightExam(rtkTime)) {
|
exam_dummy_light = 4;
|
// 汇报灯光考试结束
|
DEBUG("灯光考试结束");
|
InitRoadExam(RoadMap);
|
}
|
}
|
}
|
}
|
|
static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime)
|
{
|
static bool rec = false;
|
static bool handBreakActive = false, handBreakActive2 = false;
|
static double startCarMoveDistance;
|
static int prevMove = 0;
|
|
if (move != 0) {
|
if (ReadCarStatus(SEATBELT) == EJECT_SEATBELT && !reportSeatbeltEject) {
|
DEBUG("不系安全带");
|
reportSeatbeltEject = true;
|
AddExamFault(ExamType == TEST_TYPE_AREA? 10101: 30101, rtkTime);
|
}
|
if (rec) {
|
if (!handBreakActive2 && ReadOdo() - startCarMoveDistance >= 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) {
|
DEBUG("Handbreak active move over 10m");
|
// 手刹拉起状态下，行驶了10米以上，不合格
|
AddExamFault(40205, rtkTime);
|
} else if (handBreakActive) {
|
// 手刹拉起状态下，行驶了1米以上，扣10分
|
DEBUG("Handbreak active move over 1M");
|
AddExamFault(40206, rtkTime);
|
}
|
}
|
} else if (!handBreakActive && ReadOdo() - startCarMoveDistance >= examParam.open_door_drive_allow_distance && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
|
handBreakActive = true;
|
|
if (ExamType == TEST_TYPE_AREA) {
|
DEBUG("Handbreak active move over 1M");
|
AddExamFault(10107, rtkTime);
|
}
|
}
|
}
|
} else if (!rec || prevMove != 0) { // 记录停车点
|
rec = true;
|
handBreakActive = handBreakActive2 = false;
|
startCarMoveDistance = ReadOdo();
|
}
|
|
prevMove = move;
|
|
if (ExamType != TEST_TYPE_AREA) {
|
if (exam_dummy_light == 4 || ExamType == TEST_TYPE_ROAD_TRUE_LIGHT || ExamType == TEST_TYPE_ROAD_CALIBRATE) {
|
TestRoadGeneral(RoadMap, CarModel, CarModelList, speed, move, rtkTime);
|
}
|
} else {
|
TestAreaGeneral(AreaMapList, CarModel, CarModelList, speed, move, azimuth, rtkTime);
|
}
|
}
|
|
static void EngineStartHold(union sigval sig) {
|
AppTimer_delete(EngineStartHold);
|
DEBUG("点火超时");
|
if (ReadCarStatus(ENGINE_START) == ENGINE_START_ACTIVE) {
|
struct RtkTime rtkTime;
|
|
pthread_mutex_lock(&clock_mutex);
|
rtkTime = rtkClock;
|
pthread_mutex_unlock(&clock_mutex);
|
|
// 不及时松开启动开关，扣10分
|
if (ExamType == TEST_TYPE_AREA) {
|
AddExamFault(10201, &rtkTime);
|
} else if (ExamType != TEST_TYPE_ROAD_CALIBRATE) {
|
AddExamFault(40207, &rtkTime);
|
}
|
}
|
}
|
|
/*************************************************
|
* 2次采样相差的时间, a 最近的，b 先前的
|
* @param a
|
* @param b
|
* @return ms
|
*/
|
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b)
|
{
|
return TimeGetDiff(a->hh, a->mm, a->ss, a->dss*10, b->hh, b->mm, b->ss, b->dss*10);
|
}
|
|
static bool UpdateCarCoord(double &spd, int &mov, int &idx)
|
{
|
long tmDiff;
|
|
if (CarModel == NULL)
|
return false;
|
|
if (RtkBufferNum < 2)
|
return false;
|
|
int p1 = ((RtkBufferIn-1)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE; // 最近采样值
|
int p2 = ((RtkBufferIn-2)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE; // 找到1秒前的采样值
|
int pn = 0;
|
|
for (pn = 1; pn < RtkBufferNum; ++pn) {
|
p2 = ((RtkBufferIn-1-pn)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
|
if ((tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2])) >= D_SEC(1)) break;
|
}
|
|
if (pn == RtkBufferNum)
|
return false;
|
|
// 如果一定的时间都没有有效定位，删除之前的值
|
/*tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2]);
|
|
DEBUG("tmDiff = %ld, p1 = %d p2 = %d dss = %d dss2 = %d", tmDiff, p1, p2, RtkBuffer[p1].dss, RtkBuffer[p2].dss);
|
|
if (tmDiff > D_SEC(5)) {
|
if (p1 != 0)
|
RtkBuffer[0] = RtkBuffer[p1];
|
RtkBufferIn = RtkBufferNum = 1;
|
return false;
|
}*/
|
|
// 计算车辆轮廓点
|
PointF main_ant_coord;
|
main_ant_coord.X = RtkBuffer[p1].x;
|
main_ant_coord.Y = RtkBuffer[p1].y;
|
|
struct RtkTime tm;
|
|
tm.YY = RtkBuffer[p1].YY;
|
tm.MM = RtkBuffer[p1].MM;
|
tm.DD = RtkBuffer[p1].DD;
|
tm.hh = RtkBuffer[p1].hh;
|
tm.mm = RtkBuffer[p1].mm;
|
tm.ss = RtkBuffer[p1].ss;
|
tm.mss = RtkBuffer[p1].dss;
|
|
UpdateCarBodyCoord(&tm, RtkBuffer[p1].heading, RtkBuffer[p1].pitch, RtkBuffer[p1].roll, main_ant_coord, CarModel);
|
|
car_model *newModel = (car_model *)malloc(sizeof(car_model));
|
|
newModel->tm = CarModel->tm;
|
newModel->basePoint = CarModel->basePoint;
|
newModel->axial[0] = CarModel->axial[0];
|
newModel->axial[1] = CarModel->axial[1];
|
newModel->left_front_tire[0] = CarModel->left_front_tire[0];
|
newModel->left_front_tire[1] = CarModel->left_front_tire[1];
|
newModel->right_front_tire[0] = CarModel->right_front_tire[0];
|
newModel->right_front_tire[1] = CarModel->right_front_tire[1];
|
newModel->left_rear_tire[0] = CarModel->left_rear_tire[0];
|
newModel->left_rear_tire[1] = CarModel->left_rear_tire[1];
|
newModel->right_rear_tire[0] = CarModel->right_rear_tire[0];
|
newModel->right_rear_tire[1] = CarModel->right_rear_tire[1];
|
newModel->bodyNum = CarModel->bodyNum;
|
newModel->body = (int *) malloc(sizeof(int) * newModel->bodyNum);
|
for (int i = 0; i < newModel->bodyNum; ++i) {
|
newModel->body[i] = CarModel->body[i];
|
}
|
newModel->pointNum = CarModel->pointNum;
|
newModel->carXY = (PointF *) malloc(sizeof(PointF) * newModel->pointNum);
|
for (int i = 0; i < newModel->pointNum; ++i) {
|
newModel->carXY[i] = CarModel->carXY[i];
|
}
|
newModel->carDesc = NULL;
|
newModel->antPitch = CarModel->antPitch;
|
newModel->yaw = CarModel->yaw;
|
newModel->pitch = CarModel->pitch;
|
|
CarModelList.push_front(newModel);
|
|
while (CarModelList.size() > 100) {
|
car_model *ptr = CarModelList.back();
|
|
if (ptr->body != NULL)
|
free(ptr->body);
|
if (ptr->carXY != NULL)
|
free(ptr->carXY);
|
if (ptr->carDesc != NULL)
|
free(ptr->carDesc);
|
|
free(ptr);
|
|
CarModelList.pop_back();
|
}
|
|
// 计算速度(米/秒)、前进后退
|
double speed = sqrt(pow(RtkBuffer[p1].x - RtkBuffer[p2].x, 2) + pow(RtkBuffer[p1].y - RtkBuffer[p2].y, 2)) * 1000 /
|
(double)(tmDiff);
|
|
// DEBUG("位移 %f 时间 %ld 速度 %f", sqrt(pow(RtkBuffer[p1].x - RtkBuffer[p2].x, 2) + pow(RtkBuffer[p1].y - RtkBuffer[p2].y, 2)), tmDiff, speed);
|
// DEBUG("%d %d %f, %f - %d %d %f, %f", RtkBuffer[p1].ss, RtkBuffer[p1].dss, RtkBuffer[p1].x, RtkBuffer[p1].y, RtkBuffer[p2].ss, RtkBuffer[p2].dss, RtkBuffer[p2].x, RtkBuffer[p2].y);
|
|
int move = 0;
|
double deg = 0.0;
|
|
if (speed < 0.05) {
|
// 停车
|
move = 0;
|
} else {
|
// 判断前进还是后退
|
if (fabs(RtkBuffer[p1].y - RtkBuffer[p2].y) <= GLB_EPSILON) {
|
if (RtkBuffer[p1].x > RtkBuffer[p2].x) {
|
deg = 90;
|
} else {
|
deg = 270;
|
}
|
} else if (fabs(RtkBuffer[p1].x - RtkBuffer[p2].x) <= GLB_EPSILON) {
|
if (RtkBuffer[p1].y > RtkBuffer[p2].y) {
|
deg = 0;
|
} else {
|
deg = 180;
|
}
|
} else {
|
deg = atan(fabs(RtkBuffer[p1].x - RtkBuffer[p2].x) /
|
fabs(RtkBuffer[p1].y - RtkBuffer[p2].y));
|
|
deg = toDegree(deg);
|
|
if (RtkBuffer[p1].x > RtkBuffer[p2].x &&
|
RtkBuffer[p1].y > RtkBuffer[p2].y) {
|
|
} else if (RtkBuffer[p1].x < RtkBuffer[p2].x &&
|
RtkBuffer[p1].y > RtkBuffer[p2].y) {
|
deg = 360 - deg;
|
} else if (RtkBuffer[p1].x < RtkBuffer[p2].x &&
|
RtkBuffer[p1].y < RtkBuffer[p2].y) {
|
deg = 180 + deg;
|
} else if (RtkBuffer[p1].x > RtkBuffer[p2].x &&
|
RtkBuffer[p1].y < RtkBuffer[p2].y) {
|
deg = 180 - deg;
|
}
|
}
|
|
deg = fabs(RtkBuffer[p1].heading - deg);
|
if (deg > 180) {
|
deg = 360 - deg;
|
}
|
if (deg < 90) {
|
// 前进
|
move = 1;
|
} else {
|
// 后退
|
move = -1;
|
}
|
}
|
|
spd = speed;
|
mov = move;
|
idx = p1;
|
// DEBUG("tmDiff = %ld speed = %f m/Sec move = %d", tmDiff, speed, move);
|
|
return true;
|
}
|
|
static int currRoad = -1, currCrossing = -1;
|
|
void RoadChange(int road, int status)
|
{
|
struct roadStatusBrief brief;
|
|
brief.road_id = road;
|
brief.status = status;
|
|
MA_SendRoadStatus(&brief);
|
|
currRoad = (status == 1? road : -1);
|
|
DEBUG("报告长官 进出路段 road %d status %d", road, status);
|
}
|
|
void CrossingChange(int road, int crossing, int status)
|
{
|
struct crossingStatusBrief brief;
|
|
brief.road_id = road;
|
brief.crossing_index = crossing;
|
brief.status = status;
|
|
MA_SendCrossingStatus(&brief);
|
|
currCrossing = (status == 1? crossing : -1);
|
|
DEBUG("报告长官 进出路口 road %d crossing %d status %d", road, crossing, status);
|
}
|
|
void AddExamFault(int wrong, const struct RtkTime *rtkTime)
|
{
|
struct ExamFault fault;
|
|
if (!ExamStart)
|
return;
|
|
fault.sn = examFaultIndex++;
|
sprintf(fault.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + rtkTime->YY,
|
rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
|
|
// if (ExamType != TEST_TYPE_AREA) {
|
// wrong += 1000;
|
// }
|
|
fault.wrong_id = wrong;
|
|
DEBUG("考试发生错误 code = %d %s", wrong, fault.utc);
|
|
ExamFaultList.push_back(fault);
|
|
MA_SendExamWrong(ExamFaultList);
|
|
ExamFaultList.clear();
|
}
|
|
void MasterInqRoadStatus(void)
|
{
|
struct roadStatusBrief brief;
|
struct crossingStatusBrief brief2;
|
|
brief2.road_id = brief.road_id = currRoad;
|
|
if (currRoad >= 0) {
|
brief.status = 1;
|
|
if (currCrossing >= 0) {
|
brief2.crossing_index = currCrossing;
|
brief2.status = 1;
|
} else {
|
brief2.crossing_index = -1;
|
brief2.status = 0;
|
}
|
}
|
else {
|
brief.status = 0;
|
brief2.crossing_index = -1;
|
brief2.status = 0;
|
}
|
|
MA_SendRoadStatus(&brief);
|
|
MA_SendCrossingStatus(&brief2);
|
}
|
|
/***************************************************************
|
* 车速判断需要1秒前后的对比，实测停车确认需要1.5秒左右，固减去这个时间
|
* @param src
|
* @return
|
*/
|
int CorrectPauseCriteria(int src)
|
{
|
return (src > 1500) ? src - 1500 : 0;
|
}
|
|
/*******************************************************************
|
* @brief 由主天线坐标计算车身点坐标
|
* @param azimuth
|
* @param coord
|
*/
|
static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel)
|
{
|
// 俯仰角修正
|
// DEBUG("俯仰角 %f", pitch);
|
carModel->yaw = azimuth;
|
carModel->pitch = pitch;
|
carModel->tm = *rtkTime;
|
|
pitch = pitch - carModel->antPitch;
|
// DEBUG("yaw = %f 修正俯仰角 %f", azimuth, pitch);
|
|
// 主天线投影修正
|
carModel->basePoint.X = main_ant.X + fabs(carModel->antHeight - carModel->groundHeight) * sin(toRadians(pitch)) * sin(toRadians(azimuth));
|
carModel->basePoint.Y = main_ant.Y + fabs(carModel->antHeight - carModel->groundHeight) * sin(toRadians(pitch)) * cos(toRadians(azimuth));
|
|
for (int i = 0; i < carModel->pointNum; ++i) {
|
double qrx = carModel->carDesc[i].distance * sin(toRadians(carModel->carDesc[i].angle));
|
double qry =
|
carModel->carDesc[i].distance * cos(toRadians(carModel->carDesc[i].angle)) *
|
cos(toRadians(pitch));
|
|
double projectDistance = sqrt(pow(qrx, 2) + pow(qry, 2));
|
double projectAngle = toDegree(acos(qry / projectDistance));
|
|
if (carModel->carDesc[i].angle > 180) {
|
projectAngle = 360 - projectAngle;
|
}
|
|
// double tx = projectDistance*sin(toRadians(azimuth));
|
// double ty = projectDistance*cos(toRadians(azimuth));
|
|
carModel->carXY[i].X =
|
projectDistance * sin(toRadians(azimuth)) * cos(toRadians(projectAngle)) -
|
projectDistance * cos(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
|
carModel->basePoint.X;
|
carModel->carXY[i].Y =
|
projectDistance * sin(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
|
projectDistance * cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) +
|
carModel->basePoint.Y;
|
|
// DEBUG("<%d>. 标距 %f 标角 %f X = %f Y = %f", i, carModel->carDesc[i].distance, carModel->carDesc[i].angle,
|
// carModel->carXY[i].X, carModel->carXY[i].Y);
|
}
|
}
|
|
void SystemShutdown(int event, int timeout)
|
{
|
// 关机
|
if (event == 1) {
|
ShutdownInd(timeout);
|
}
|
// 重启
|
if (event == 0) {
|
|
}
|
}
|
|
void SensorXChanged(uint16_t id, int value)
|
{
|
handlePrepare(id, value);
|
handleLigthExam(id, value);
|
}
|
|
void PrepareOver(int res)
|
{
|
DEBUG("上车准备结束 %d", res);
|
|
if (res != 0) {
|
struct RtkTime rtkTime;
|
|
pthread_mutex_lock(&clock_mutex);
|
rtkTime = rtkClock;
|
pthread_mutex_unlock(&clock_mutex);
|
|
AddExamFault(40101, &rtkTime);
|
}
|
exam_dummy_light = 2;
|
}
|
