#include "dcm.h" #include "facilities.h" #include #include #include #include #include #include #define SLEEP_TIME_MS 100 static int are_paths_intersecting(it2s_tender_st_s* tA, int tA_len, it2s_tender_st_s* tB, int tB_len) { // TODO check first time intersection, then the spacial trajectory double A1[2], A2[2], B1[2], B2[2]; uint64_t tsA, tsB; for (int a = 0; a < tA_len-1; ++a) { A1[0] = tA[a].latitude; A1[1] = tA[a].longitude; A2[0] = tA[a+1].latitude; A2[1] = tA[a+1].longitude; for (int b = 0; b < tB_len-1; ++b) { B1[0] = tB[b].latitude; B1[1] = tB[b].longitude; B2[0] = tB[b+1].latitude; B2[1] = tB[b+1].longitude; if (it2s_tender_do_segments_intersect(A1, A2, B1, B2)) { if (tA[a].timestamp < tB[b].timestamp + 2000 && tA[a].timestamp > tB[b].timestamp - 2000) { syslog_info("[facilities] [dc] intersecting @ (%d, %d) in %ld ms", tA[a].latitude, tA[a].longitude, tA[a].timestamp-tA[0].timestamp); return 1; } } } } return 0; } int dcm_check(void* fc, DCM_t* dcm) { facilities_t* facilities = (facilities_t*) fc; it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */ it2s_tender_st_s trajectoryB[TRAJECTORY_MAX_LEN+1]; /* neighbour trajectory */ uint16_t trajectoryA_len = 0; uint16_t trajectoryB_len = 0; uint64_t now = it2s_tender_get_clock(&facilities->epv); int32_t lat, lon; it2s_tender_lock_space(&facilities->epv); it2s_tender_get_space(&facilities->epv); lat = facilities->epv.space.latitude; lon = facilities->epv.space.longitude; it2s_tender_unlock_space(&facilities->epv); it2s_tender_lock_trajectory(&facilities->epv); trajectoryA_len = facilities->epv.trajectory.len; memcpy(trajectoryA + 1, facilities->epv.trajectory.path, trajectoryA_len * sizeof(it2s_tender_st_s)); it2s_tender_unlock_trajectory(&facilities->epv); trajectoryA[0].latitude = lat; trajectoryA[0].longitude = lon; trajectoryA[0].timestamp = now; ++trajectoryA_len; trajectoryB[0].latitude = dcm->dcm.currentPosition.latitude; trajectoryB[0].longitude = dcm->dcm.currentPosition.longitude; asn_INTEGER2ulong(&dcm->dcm.currentPosition.timestamp, &trajectoryB[0].timestamp); ++trajectoryB_len; for (int i = 0; i < dcm->dcm.plannedTrajectory.list.count && i < TRAJECTORY_MAX_LEN; ++i) { trajectoryB[i+1].latitude = dcm->dcm.plannedTrajectory.list.array[i]->latitude; trajectoryB[i+1].longitude = dcm->dcm.plannedTrajectory.list.array[i]->longitude; asn_INTEGER2ulong(&dcm->dcm.plannedTrajectory.list.array[i]->timestamp, &trajectoryB[i+1].timestamp); ++trajectoryB_len; } if (trajectoryA_len > 1 && trajectoryB_len > 1) { are_paths_intersecting(trajectoryA, trajectoryA_len, trajectoryB, trajectoryB_len); } cleanup: return 0; } static int mk_dcm(facilities_t* facilities, uint8_t* dcm_uper, uint16_t* dcm_uper_len) { int rv = 0; it2s_tender_st_s trajectory[TRAJECTORY_MAX_LEN]; DCM_t* dcm = calloc(1, sizeof(DCM_t)); dcm->header.messageID = 43; dcm->header.protocolVersion = 1; uint64_t now = it2s_tender_get_clock(&facilities->epv); pthread_mutex_lock(&facilities->id.lock); dcm->header.stationID = facilities->id.station_id; pthread_mutex_unlock(&facilities->id.lock); int32_t lat, lon; uint16_t trajectory_len = 0; it2s_tender_lock_space(&facilities->epv); it2s_tender_get_space(&facilities->epv); lat = facilities->epv.space.latitude; lon = facilities->epv.space.longitude; it2s_tender_unlock_space(&facilities->epv); it2s_tender_lock_trajectory(&facilities->epv); trajectory_len = facilities->epv.trajectory.len; memcpy(trajectory, facilities->epv.trajectory.path, trajectory_len * sizeof(it2s_tender_st_s)); it2s_tender_unlock_trajectory(&facilities->epv); dcm->dcm.currentPosition.latitude = lat; dcm->dcm.currentPosition.longitude = lon; asn_ulong2INTEGER(&dcm->dcm.currentPosition.timestamp, now); // Vehicle Dimensions dcm->dcm.vehicleDimensions.length.vehicleLengthValue = facilities->vehicle.length; dcm->dcm.vehicleDimensions.length.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_noTrailerPresent; dcm->dcm.vehicleDimensions.width = facilities->vehicle.width; dcm->dcm.plannedTrajectory.list.count = trajectory_len; dcm->dcm.plannedTrajectory.list.size = trajectory_len * sizeof(void*); dcm->dcm.plannedTrajectory.list.array = malloc(trajectory_len * sizeof(void*)); for (int i = 0; i < trajectory_len; ++i) { dcm->dcm.plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); dcm->dcm.plannedTrajectory.list.array[i]->latitude = trajectory[i].latitude; dcm->dcm.plannedTrajectory.list.array[i]->longitude = trajectory[i].longitude; asn_ulong2INTEGER(&dcm->dcm.plannedTrajectory.list.array[i]->timestamp, trajectory[i].timestamp); } asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_DCM, NULL, dcm, dcm_uper, 512); if (enc.encoded == -1) { syslog_err("[facilities] [dc] failed encoding DCM (%s)", enc.failed_type->name); rv = 1; goto cleanup; } *dcm_uper_len = (enc.encoded + 7) / 8; cleanup: ASN_STRUCT_FREE(asn_DEF_DCM, dcm); return rv; } void* dc_service(void* fc) { facilities_t* facilities = (facilities_t*) fc; uint8_t dcm[512]; TransportRequest_t* tr = calloc(1, sizeof(TransportRequest_t)); tr->present = TransportRequest_PR_packet; tr->choice.packet.present = TransportPacketRequest_PR_btp; BTPPacketRequest_t* bpr = &tr->choice.packet.choice.btp; bpr->btpType = BTPType_btpB; bpr->gn.destinationAddress.buf = malloc(6); for (int i = 0; i < 6; ++i) { bpr->gn.destinationAddress.buf[i] = 0xff; } bpr->gn.destinationAddress.size = 6; bpr->gn.packetTransportType = PacketTransportType_shb; bpr->destinationPort = 2043; bpr->gn.trafficClass = 2; bpr->data.buf = malloc(512); uint8_t tr_oer[1024]; tr_oer[0] = 4; int rv; while (!facilities->exit) { rv = mk_dcm(facilities, bpr->data.buf, (uint16_t *) &bpr->data.size); if (rv) { continue; } asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer+1, 1023); if (enc.encoded == -1) { syslog_err("[facilities] encoding TR for cam failed"); continue; } queue_send(facilities->tx_queue, tr_oer, enc.encoded+1, 3); usleep(SLEEP_TIME_MS * 1000); } return NULL; }