199 lines
6.5 KiB
C
199 lines
6.5 KiB
C
#include "dcm.h"
|
|
#include "facilities.h"
|
|
|
|
#include <it2s-tender/epv.h>
|
|
#include <it2s-tender/space.h>
|
|
#include <it2s-tender/trajectory.h>
|
|
#include <it2s-tender/geodesy.h>
|
|
#include <itss-transport/TransportRequest.h>
|
|
#include <dcm/DCM.h>
|
|
|
|
#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 + sizeof(it2s_tender_st_s), 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);
|
|
|
|
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;
|
|
}
|