| | |
| | | |
| | | 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) |
| | | int *body, int bodyNum, double (*point)[2], int pointNum, double antPitch, double antHeight, double groundHeight) |
| | | { |
| | | DEBUG("加入车辆信息 pointNum %d", pointNum); |
| | | |
| | |
| | | } |
| | | } |
| | | |
| | | CarModel->antPitch = -0.6;//antPitch;///////////////////////////////////// |
| | | CarModel->antPitch = antPitch; |
| | | CarModel->antHeight = antHeight; |
| | | CarModel->groundHeight = groundHeight; |
| | | |
| | | CarModel->pointNum = pointNum; |
| | | CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * pointNum); |
| | |
| | | |
| | | static void ExecuteExam(const struct RtkTime* rtkTime) |
| | | { |
| | | // DEBUG("ExecuteExam 111111111"); |
| | | { |
| | | static const char *NAME[] = {"OBD_SPEED", |
| | | "ENGINE_RPM", |
| | |
| | | "CAR_STATUS_END"}; |
| | | |
| | | static const char *VALUE[] = { |
| | | "0", |
| | | "INACTIVE", |
| | | "HAZARD_LIGHTS", |
| | | "LEFT_TURN_LIGHT", |
| | | "RIGHT_TURN_LIGHT", |
| | |
| | | } |
| | | |
| | | } |
| | | //DEBUG("-------------- 1111111111"); |
| | | |
| | | if (ReadCarStatus(ENGINE_RPM) < ENGINE_MIN_ROTATE) { |
| | | if (engineRuning) { |
| | |
| | | } |
| | | } |
| | | } |
| | | |
| | | // DEBUG("ExecuteExam 1111111111 ----------------------"); |
| | | } |
| | | |
| | | static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime) |
| | | { |
| | | // DEBUG("ExecuteExam 2222222"); |
| | | if (move != 0) { |
| | | if (ReadCarStatus(SEATBELT) == EJECT_SEATBELT && !reportSeatbeltEject) { |
| | | DEBUG("不系安全带"); |
| | |
| | | CurrExamMapIndex = -1; |
| | | } |
| | | } |
| | | // DEBUG("ExecuteExam 2222222 -----------------------"); |
| | | } |
| | | |
| | | static void EngineStartHold(union sigval sig) { |
| | |
| | | Line carAxial; |
| | | |
| | | MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]); |
| | | MakeLine(&carAxial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]); |
| | | MakeLine(&carAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]); |
| | | |
| | | if (IntersectionOf(startLine, carAxial) == GM_Intersection) { |
| | | if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], startLine) > 0.1) |
| | | if (DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], startLine) > 0.1) |
| | | return i; |
| | | } |
| | | } |
| | |
| | | */ |
| | | static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel) |
| | | { |
| | | carModel->basePoint = main_ant; |
| | | |
| | | |
| | | // 俯仰角修正 |
| | | // DEBUG("俯仰角 %f", pitch); |
| | | carModel->yaw = azimuth; |
| | | carModel->pitch = pitch; |
| | | carModel->tm = *rtkTime; |
| | | |
| | | pitch = pitch - carModel->antPitch; |
| | | // DEBUG("yaw = %f 修正俯仰角 %f", azimuth, pitch); |
| | | |
| | | // DEBUG("xz俯仰角 %f", 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)) / |
| | | carModel->carDesc[i].distance * cos(toRadians(carModel->carDesc[i].angle)) * |
| | | cos(toRadians(pitch)); |
| | | |
| | | double projectDistance = sqrt(pow(qrx, 2) + pow(qry, 2)); |
| | |
| | | carModel->carXY[i].X = |
| | | projectDistance * sin(toRadians(azimuth)) * cos(toRadians(projectAngle)) - |
| | | projectDistance * cos(toRadians(azimuth)) * sin(toRadians(projectAngle)) + |
| | | main_ant.X; |
| | | carModel->basePoint.X; |
| | | carModel->carXY[i].Y = |
| | | projectDistance * sin(toRadians(azimuth)) * sin(toRadians(projectAngle)) + |
| | | projectDistance * cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) + |
| | | main_ant.Y; |
| | | 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); |
| | | } |
| | | } |
| | | |