//
|
// Created by YY on 2020/3/23.
|
//
|
|
#include "drive_straight.h"
|
#include "../driver_test.h"
|
#include "../native-lib.h"
|
#include "../jni_log.h"
|
#include "road_exam.h"
|
#include "../master/comm_if.h"
|
#include "../common/apptimer.h"
|
#include "../test_common/odo_graph.h"
|
#include <cmath>
|
|
#define DEBUG(fmt, args...) LOGD("<drive_straight> <%s>: " fmt, __func__, ##args)
|
|
static const double CHECK_STAGE_DISTANCE = 100.0;
|
static const double MAX_OFFSET_DISTANCE = 0.3;
|
|
static int setup;
|
static double beginOdo;
|
static int yaw_stat;
|
|
static void TtsBack(int seq)
|
{
|
setup = 1;
|
}
|
|
void StartDriveStraightExam(void) {
|
DEBUG("开始直线行驶");
|
setup = 0;
|
yaw_stat = 0;
|
PlayTTS("请保持直线行驶", TtsBack);
|
}
|
|
bool TestDriveStraight(const car_model *car, const struct RtkTime *rtkTime) {
|
static double yaw;
|
static Line baseLine;
|
|
if (setup == 1) {
|
// 偏航角平均值
|
if (yaw_stat == 0) {
|
yaw = car->yaw;
|
} else {
|
yaw += car->yaw;
|
}
|
yaw_stat++;
|
|
DEBUG("角度矫正 car %f yaw %f", car->yaw, yaw);
|
|
if (yaw_stat == 5) {
|
yaw = fmod(yaw, 360) / 5;
|
DEBUG("创建基线 yaw %f", yaw);
|
PointF extPoint = PointExtend(car->basePoint, 100, yaw);
|
MakeLine(&baseLine, &car->basePoint, &extPoint);
|
beginOdo = ReadOdo();
|
setup = 2;
|
}
|
} else if (setup == 2) {
|
double offset = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine);
|
|
if (offset > MAX_OFFSET_DISTANCE) {
|
DEBUG("直线偏移大于30厘米 offset = %f", offset);
|
// 偏移大于30厘米，不合格
|
AddExamFault(30, rtkTime);
|
return false;
|
}
|
}
|
|
if (setup == 2 && ReadOdo() - beginOdo > CHECK_STAGE_DISTANCE) {
|
DEBUG("直线行驶结束");
|
PlayTTS("直线行驶结束", NULL);
|
return false;
|
}
|
|
return true;
|
}
|
