| | |
| | | } |
| | | } |
| | | |
| | | CarModel->antPitch = antPitch; |
| | | CarModel->antPitch = -0.6;//antPitch;///////////////////////////////////// |
| | | |
| | | 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", |
| | |
| | | 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 < CAR_STATUS_END; ++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] ]); |
| | | |
| | |
| | | } |
| | | |
| | | } |
| | | |
| | | //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) { |
| | |
| | | { |
| | | carModel->basePoint = main_ant; |
| | | |
| | | |
| | | // DEBUG("俯仰角 %f", pitch); |
| | | carModel->yaw = azimuth; |
| | | carModel->pitch = pitch; |
| | | carModel->tm = *rtkTime; |
| | | |
| | | pitch = pitch - carModel->antPitch; |
| | | |
| | | // DEBUG("xz俯仰角 %f", pitch); |
| | | |
| | | 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)); |