TiagoRG
/
it2s-itss-facilities
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Shift to gnss-based positioning

This commit is contained in:
emanuel 2024-02-27 16:13:53 +00:00
parent f145f296aa
commit da033be14f
11 changed files with 194 additions and 259 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitlab-ci.yml
View File

@ -8,7 +8,7 @@ stages:
#.dependencies:
# before_script:
# - pacman -Sy zeromq it2s-config-git it2s-asn-git it2s-tender-git mariadb it2s-gps-git it2s-obd-git --overwrite=* --noconfirm
# - pacman -Sy zeromq it2s-config-git it2s-asn-git it2s-tender-git mariadb it2s-gnss-git it2s-obd-git --overwrite=* --noconfirm
#build:
# stage: build

66
src/cam.c
View File

@ -183,25 +183,25 @@ static int mk_cam_v1(uint8_t *cam_oer, uint16_t *cam_len) {
itss_space_lock();
itss_space_get();
bc->referencePosition.altitude.altitudeValue = epv.space.altitude;
bc->referencePosition.altitude.altitudeConfidence = epv.space.altitude_conf;
bc->referencePosition.altitude.altitudeValue = epv.space.data.altitude.value;
bc->referencePosition.altitude.altitudeConfidence = epv.space.data.altitude.confidence;
// Set GPS coordinates
bc->referencePosition.latitude = epv.space.latitude;
bc->referencePosition.longitude = epv.space.longitude;
uint16_t lat_conf = epv.space.latitude_conf;
uint16_t lon_conf = epv.space.longitude_conf;
bc->referencePosition.latitude = epv.space.data.latitude.value;
bc->referencePosition.longitude = epv.space.data.longitude.value;
uint16_t lat_conf = epv.space.data.latitude.confidence;
uint16_t lon_conf = epv.space.data.longitude.confidence;
cam->cam.camParameters.highFrequencyContainer.present = EI1_HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
EI1_BasicVehicleContainerHighFrequency_t* bvc_hf = &cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
// Set speed
bvc_hf->speed.speedValue = epv.space.speed;
bvc_hf->speed.speedConfidence = epv.space.speed_conf;
bvc_hf->speed.speedValue = epv.space.data.speed.value;
bvc_hf->speed.speedConfidence = epv.space.data.speed.confidence;
// Set heading
bvc_hf->heading.headingValue = epv.space.heading;
bvc_hf->heading.headingConfidence = epv.space.heading_conf;
bvc_hf->heading.headingValue = epv.space.data.heading.value;
bvc_hf->heading.headingConfidence = epv.space.data.heading.confidence;
itss_space_unlock(epv);
@ -342,12 +342,12 @@ static int mk_cam_v1(uint8_t *cam_oer, uint16_t *cam_len) {
itss_space_lock();
itss_space_get();
bc->referencePosition.altitude.altitudeValue = epv.space.altitude;
bc->referencePosition.altitude.altitudeConfidence = epv.space.altitude_conf;
bc->referencePosition.altitude.altitudeValue = epv.space.data.altitude.value;
bc->referencePosition.altitude.altitudeConfidence = epv.space.data.altitude.confidence;
// Set GPS coordinates
bc->referencePosition.latitude = epv.space.latitude;
bc->referencePosition.longitude = epv.space.longitude;
bc->referencePosition.latitude = epv.space.data.latitude.value;
bc->referencePosition.longitude = epv.space.data.longitude.value;
bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = EI1_SemiAxisLength_unavailable;
bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = EI1_SemiAxisLength_unavailable;
bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = EI1_HeadingValue_unavailable;
@ -427,25 +427,25 @@ static int mk_cam_v2(uint8_t *cam_oer, uint16_t *cam_len) {
itss_space_lock();
itss_space_get();
bc->referencePosition.altitude.altitudeValue = epv.space.altitude;
bc->referencePosition.altitude.altitudeConfidence = epv.space.altitude_conf;
bc->referencePosition.altitude.altitudeValue = epv.space.data.altitude.value;
bc->referencePosition.altitude.altitudeConfidence = epv.space.data.altitude.confidence;
// Set GPS coordinates
bc->referencePosition.latitude = epv.space.latitude;
bc->referencePosition.longitude = epv.space.longitude;
uint16_t lat_conf = epv.space.latitude_conf;
uint16_t lon_conf = epv.space.longitude_conf;
bc->referencePosition.latitude = epv.space.data.latitude.value;
bc->referencePosition.longitude = epv.space.data.longitude.value;
uint16_t lat_conf = epv.space.data.latitude.confidence;
uint16_t lon_conf = epv.space.data.longitude.confidence;
cam->cam.camParameters.highFrequencyContainer.present = EI2_HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
EI2_BasicVehicleContainerHighFrequency_t* bvc_hf = &cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
// Set speed
bvc_hf->speed.speedValue = epv.space.speed;
bvc_hf->speed.speedConfidence = epv.space.speed_conf;
bvc_hf->speed.speedValue = epv.space.data.speed.value;
bvc_hf->speed.speedConfidence = epv.space.data.speed.confidence;
// Set heading
bvc_hf->heading.headingValue = epv.space.heading;
bvc_hf->heading.headingConfidence = epv.space.heading_conf;
bvc_hf->heading.headingValue = epv.space.data.heading.value;
bvc_hf->heading.headingConfidence = epv.space.data.heading.confidence;
itss_space_unlock(epv);
@ -587,12 +587,12 @@ static int mk_cam_v2(uint8_t *cam_oer, uint16_t *cam_len) {
itss_space_lock();
itss_space_get();
bc->referencePosition.altitude.altitudeValue = epv.space.altitude;
bc->referencePosition.altitude.altitudeConfidence = epv.space.altitude_conf;
bc->referencePosition.altitude.altitudeValue = epv.space.data.altitude.value;
bc->referencePosition.altitude.altitudeConfidence = epv.space.data.altitude.confidence;
// Set GPS coordinates
bc->referencePosition.latitude = epv.space.latitude;
bc->referencePosition.longitude = epv.space.longitude;
bc->referencePosition.latitude = epv.space.data.latitude.value;
bc->referencePosition.longitude = epv.space.data.longitude.value;
bc->referencePosition.positionConfidenceEllipse.semiMinorAxisLength = EI2_SemiAxisLength_unavailable;
bc->referencePosition.positionConfidenceEllipse.semiMajorAxisLength = EI2_SemiAxisLength_unavailable;
bc->referencePosition.positionConfidenceEllipse.semiMajorAxisOrientation = EI2_HeadingValue_unavailable;
@ -680,18 +680,18 @@ int lightship_check() {
itss_space_get();
// Check heading delta > 4º
int diff = epv.space.heading - lightship->last_pos.heading;
int diff = epv.space.data.heading.value - lightship->last_pos.heading;
if (abs(diff) > 40) rv = 1;
if (!rv) {
// Check speed delta > 0.5 m/s
diff = epv.space.speed - lightship->last_pos.speed;
diff = epv.space.data.speed.value - lightship->last_pos.speed;
if (abs(diff) > 50) rv = 1;
if (!rv) {
// Check position delta > 4 m
// TODO make an *accurate* distance calculator using GPS coords
int32_t avg_speed = (epv.space.speed + lightship->last_pos.speed)/2 / 100; /* cm/s to m/s */
int32_t avg_speed = (epv.space.data.speed.value + lightship->last_pos.speed)/2 / 100; /* cm/s to m/s */
uint64_t delta_time = (now - lightship->t_last_cam) / 1000; /* ms to s */
if (avg_speed * delta_time > 4) rv = 1;
}
@ -1173,8 +1173,8 @@ static int check_pz() {
itss_space_lock();
itss_space_get();
double lat = epv.space.latitude/10000000.0;
double lon = epv.space.longitude/10000000.0;
double lat = epv.space.data.latitude.value/10000000.0;
double lon = epv.space.data.longitude.value/10000000.0;
itss_space_unlock();
pthread_mutex_lock(&lightship->lock);

42
src/config.c
View File

@ -502,16 +502,9 @@ int facilities_config() {
EIS_ManagementRequest_t* mreq = calloc(1, sizeof(EIS_ManagementRequest_t));
mreq->present = EIS_ManagementRequest_PR_attributes;
mreq->choice.attributes.present = EIS_ManagementRequestAttributes_PR_get;
mreq->choice.attributes.choice.get.coordinates = 1;
mreq->choice.attributes.choice.get.altitude = 1;
mreq->choice.attributes.choice.get.heading = 1;
mreq->choice.attributes.choice.get.speed = 1;
mreq->choice.attributes.choice.get.acceleration = 1;
mreq->choice.attributes.choice.get.gpsType = 1;
mreq->choice.attributes.choice.get.clockType = 1;
mreq->choice.attributes.choice.get.clock = 1;
mreq->choice.attributes.choice.get.clockOffset = 1;
mreq->choice.attributes.choice.get.trajectory = etsi_its_cfg->facilities.mcm.activate;
void* management_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ);
uint8_t b_tx[256], b_rx[256];
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_ManagementRequest, NULL, mreq, b_tx, 256);
@ -529,42 +522,13 @@ int facilities_config() {
long lat, lon, alt, alt_conf;
if (mrep->returnCode == EIS_ManagementReplyReturnCode_accepted &&
mrep->data &&
mrep->data->choice.attributes.coordinates &&
mrep->data->choice.attributes.altitude &&
mrep->data->choice.attributes.heading &&
mrep->data->choice.attributes.speed &&
mrep->data->choice.attributes.acceleration &&
mrep->data->choice.attributes.clockType &&
mrep->data->choice.attributes.clock &&
mrep->data->choice.attributes.clockOffset &&
mrep->data->choice.attributes.gpsType &&
(!!mrep->data->choice.attributes.trajectory == mreq->choice.attributes.choice.get.trajectory)) {
epv.space.latitude = mrep->data->choice.attributes.coordinates->latitude;
epv.space.latitude_conf = mrep->data->choice.attributes.coordinates->latitudeConfidence;
epv.space.longitude = mrep->data->choice.attributes.coordinates->longitude;
epv.space.longitude_conf = mrep->data->choice.attributes.coordinates->longitudeConfidence;
epv.space.altitude = mrep->data->choice.attributes.altitude->altitudeValue;
epv.space.altitude_conf = mrep->data->choice.attributes.altitude->altitudeConfidence;
epv.space.heading = mrep->data->choice.attributes.heading->headingValue;
epv.space.heading_conf = mrep->data->choice.attributes.heading->headingConfidence;
epv.space.speed = mrep->data->choice.attributes.speed->speedValue;
epv.space.speed_conf = mrep->data->choice.attributes.speed->speedConfidence;
epv.space.accel = mrep->data->choice.attributes.acceleration->longitudinalAccelerationValue;
epv.space.type = *mrep->data->choice.attributes.gpsType;
mrep->data->choice.attributes.clockOffset) {
epv.time.type = *mrep->data->choice.attributes.clockType;
asn_INTEGER2ulong(mrep->data->choice.attributes.clock, &epv.time.clock);
asn_INTEGER2ulong(mrep->data->choice.attributes.clockOffset, &epv.time.offset);
if (etsi_its_cfg->facilities.mcm.activate) {
epv.trajectory.len = mrep->data->choice.attributes.trajectory->list.count;
for (int i = 0; i < mrep->data->choice.attributes.trajectory->list.count; ++i) {
epv.trajectory.path[i].latitude = mrep->data->choice.attributes.trajectory->list.array[i]->latitude;
epv.trajectory.path[i].longitude = mrep->data->choice.attributes.trajectory->list.array[i]->longitude;
asn_INTEGER2ulong(&mrep->data->choice.attributes.trajectory->list.array[i]->timestamp, &epv.trajectory.path[i].timestamp);
}
}
asn_INTEGER2uint64(mrep->data->choice.attributes.clock, &epv.time.clock);
asn_INTEGER2uint64(mrep->data->choice.attributes.clockOffset, &epv.time.offset);
} else {
log_error("rejected MR attribute request");
rv = 1;

8
src/cpm.c
View File

@ -572,10 +572,10 @@ static int mk_cpm(uint8_t *bdr_oer, uint32_t *bdr_len, uint8_t *fdi_oer, uint32_
int32_t lat, lon, alt, alt_conf;
itss_space_lock();
itss_space_get();
lat = epv.space.latitude;
lon = epv.space.longitude;
alt = epv.space.altitude;
alt_conf = epv.space.altitude_conf;
lat = epv.space.data.latitude.value;
lon = epv.space.data.longitude.value;
alt = epv.space.data.altitude.value;
alt_conf = epv.space.data.altitude.confidence;
itss_space_unlock();
cpm_tx->cpm.generationDeltaTime = generationDeltaTime;

4
src/evm.c
View File

@ -61,8 +61,8 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) {
itss_space_get();
cs0->chargingStationLocation.latitude = 405970830;
cs0->chargingStationLocation.longitude = -86628610;
cs0->chargingStationLocation.altitude.altitudeValue = epv.space.altitude;
cs0->chargingStationLocation.altitude.altitudeConfidence = epv.space.altitude_conf;
cs0->chargingStationLocation.altitude.altitudeValue = epv.space.data.altitude.value;
cs0->chargingStationLocation.altitude.altitudeConfidence = epv.space.data.altitude.confidence;
cs0->chargingStationLocation.positionConfidenceEllipse.semiMajorConfidence = EI1_SemiAxisLength_unavailable;
cs0->chargingStationLocation.positionConfidenceEllipse.semiMinorConfidence = EI1_SemiAxisLength_unavailable;
cs0->chargingStationLocation.positionConfidenceEllipse.semiMajorOrientation = EI1_HeadingValue_unavailable;

25
src/facilities.c
View File

@ -274,31 +274,6 @@ static int management_indication(void *responder, uint8_t *msg, uint32_t msg_len
itss_0send(responder, &code, 1);
if (mi->present == EIS_ManagementIndication_PR_attributes) {
itss_space_lock();
epv.space.latitude = mi->choice.attributes.coordinates.latitude;
epv.space.latitude_conf = mi->choice.attributes.coordinates.latitudeConfidence;
epv.space.longitude = mi->choice.attributes.coordinates.longitude;
epv.space.longitude_conf = mi->choice.attributes.coordinates.longitudeConfidence;
epv.space.speed = mi->choice.attributes.speed.speedValue;
epv.space.speed_conf = mi->choice.attributes.speed.speedConfidence;
epv.space.heading = mi->choice.attributes.heading.headingValue;
epv.space.heading_conf = mi->choice.attributes.heading.headingConfidence;
epv.space.accel = mi->choice.attributes.acceleration.longitudinalAccelerationValue;
epv.space.altitude = mi->choice.attributes.altitude.altitudeValue;
epv.space.altitude_conf = mi->choice.attributes.altitude.altitudeConfidence;
itss_space_unlock();
itss_trajectory_lock();
if (mi->choice.attributes.trajectory) {
epv.trajectory.len = mi->choice.attributes.trajectory->list.count;
for (int i = 0; i < mi->choice.attributes.trajectory->list.count; ++i) {
epv.trajectory.path[i].latitude = mi->choice.attributes.trajectory->list.array[i]->latitude;
epv.trajectory.path[i].longitude = mi->choice.attributes.trajectory->list.array[i]->longitude;
asn_INTEGER2ulong(&mi->choice.attributes.trajectory->list.array[i]->timestamp, &epv.trajectory.path[i].timestamp);
}
}
itss_trajectory_unlock();
itss_time_lock();
asn_INTEGER2ulong(&mi->choice.attributes.clock, &epv.time.clock);
itss_time_unlock();

12
src/requests.c
View File

@ -241,20 +241,20 @@ int facilities_request_single_message(void *responder, EIS_FacilitiesMessageRequ
ph->list.array[0] = calloc(1, sizeof(EI1_PathPoint_t));
itss_space_lock();
if (path_history[0]->alt != EI1_AltitudeValue_unavailable && epv.space.altitude != EI1_AltitudeValue_unavailable) {
ph->list.array[0]->pathPosition.deltaAltitude = path_history[0]->alt - epv.space.altitude;
if (path_history[0]->alt != EI1_AltitudeValue_unavailable && epv.space.data.altitude.value != EI1_AltitudeValue_unavailable) {
ph->list.array[0]->pathPosition.deltaAltitude = path_history[0]->alt - epv.space.data.altitude.value;
} else {
ph->list.array[0]->pathPosition.deltaAltitude = EI1_DeltaAltitude_unavailable;
}
if (path_history[0]->lat != EI1_Latitude_unavailable && epv.space.latitude != EI1_Latitude_unavailable) {
ph->list.array[0]->pathPosition.deltaLatitude = path_history[0]->lat - epv.space.latitude;
if (path_history[0]->lat != EI1_Latitude_unavailable && epv.space.data.latitude.value != EI1_Latitude_unavailable) {
ph->list.array[0]->pathPosition.deltaLatitude = path_history[0]->lat - epv.space.data.latitude.value;
} else {
ph->list.array[0]->pathPosition.deltaLatitude = EI1_DeltaLatitude_unavailable;
}
if (path_history[0]->lon != EI1_Longitude_unavailable && epv.space.longitude != EI1_Longitude_unavailable) {
ph->list.array[0]->pathPosition.deltaLongitude = path_history[0]->lon - epv.space.longitude;
if (path_history[0]->lon != EI1_Longitude_unavailable && epv.space.data.longitude.value != EI1_Longitude_unavailable) {
ph->list.array[0]->pathPosition.deltaLongitude = path_history[0]->lon - epv.space.data.longitude.value;
} else {
ph->list.array[0]->pathPosition.deltaLongitude = EI1_DeltaLongitude_unavailable;
}

4
src/saem.c
View File

@ -381,8 +381,8 @@ void *sa_service() {
int32_t lat, lon;
itss_space_lock();
itss_space_get();
lat = epv.space.latitude;
lon = epv.space.longitude;
lat = epv.space.data.latitude.value;
lon = epv.space.data.longitude.value;
itss_space_unlock();
uint64_t now = itss_time_get();

30
src/tpm.c
View File

@ -28,9 +28,9 @@ int tpm_is_inside_zone(tolling_info_t* ti) {
int16_t heading;
itss_space_lock();
itss_space_get();
point[0] = epv.space.latitude/1.0e7;
point[1] = epv.space.longitude/1.0e7;
heading = epv.space.heading;
point[0] = epv.space.data.latitude.value/1.0e7;
point[1] = epv.space.data.longitude.value/1.0e7;
heading = epv.space.data.heading.value;
itss_space_unlock();
if (itss_is_inside_polygon(point, ti->zone.polygon, ti->zone.polygon_len)) {
@ -111,12 +111,12 @@ int tpm_pay(tolling_info_t* info, void** security_socket, uint8_t* neighbour, ui
// referencePosition
itss_space_lock();
itss_space_get();
tpm->tpm->referencePosition.altitude.altitudeValue = epv.space.altitude;
tpm->tpm->referencePosition.altitude.altitudeConfidence = epv.space.altitude_conf;
tpm->tpm->referencePosition.latitude = epv.space.latitude;
tpm->tpm->referencePosition.longitude = epv.space.longitude;
uint16_t lat_conf = epv.space.latitude_conf;
uint16_t lon_conf = epv.space.longitude_conf;
tpm->tpm->referencePosition.altitude.altitudeValue = epv.space.data.altitude.value;
tpm->tpm->referencePosition.altitude.altitudeConfidence = epv.space.data.altitude.confidence;
tpm->tpm->referencePosition.latitude = epv.space.data.latitude.value;
tpm->tpm->referencePosition.longitude = epv.space.data.longitude.value;
uint16_t lat_conf = epv.space.data.latitude.confidence;
uint16_t lon_conf = epv.space.data.longitude.confidence;
itss_space_unlock();
if (lat_conf > lon_conf) {
tpm->tpm->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf;
@ -708,12 +708,12 @@ static int rsu_handle_recv(EI1_TPM_t* tpm_rx, void** security_socket, uint8_t* n
// referencePosition
itss_space_lock();
itss_space_get();
tpm->tpm->referencePosition.altitude.altitudeValue = epv.space.altitude;
tpm->tpm->referencePosition.altitude.altitudeConfidence = epv.space.altitude_conf;
tpm->tpm->referencePosition.latitude = epv.space.latitude;
tpm->tpm->referencePosition.longitude = epv.space.longitude;
uint16_t lat_conf = epv.space.latitude_conf;
uint16_t lon_conf = epv.space.longitude_conf;
tpm->tpm->referencePosition.altitude.altitudeValue = epv.space.data.altitude.value;
tpm->tpm->referencePosition.altitude.altitudeConfidence = epv.space.data.altitude.confidence;
tpm->tpm->referencePosition.latitude = epv.space.data.latitude.value;
tpm->tpm->referencePosition.longitude = epv.space.data.longitude.value;
uint16_t lat_conf = epv.space.data.latitude.confidence;
uint16_t lon_conf = epv.space.data.longitude.confidence;
itss_space_unlock();
if (lat_conf > lon_conf) {
tpm->tpm->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf;

252
src/vcm.c
View File

@ -21,6 +21,7 @@ static int do_paths_intersect(
itss_st_t* tB, int tB_len, uint16_t vB_length, uint16_t vB_width,
int* index
) {
/* TODO this can be made way more efficient */
double A1[2], A2[2], B1[2], B2[2];
uint64_t tsA, tsB;
@ -178,19 +179,20 @@ static void vcm_reject(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
int32_t lat, lon;
uint16_t heading;
uint8_t heading_conf;
GNSSDeltaPosition dpA[TRAJECTORY_MAX_LEN];
int dpA_len;
itss_space_lock();
itss_space_get();
lat = epv.space.latitude;
lon = epv.space.longitude;
heading = epv.space.heading;
heading_conf = epv.space.heading_conf;
lat = epv.space.data.latitude.value;
lon = epv.space.data.longitude.value;
heading = epv.space.data.heading.value;
heading_conf = epv.space.data.heading.confidence;
dpA_len = epv.space.data.ptraj_len;
memcpy(dpA, epv.space.data.ptraj, dpA_len * sizeof(GNSSDeltaPosition));
itss_space_unlock();
uint64_t now = itss_time_get();
itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
ssize_t trajectoryA_len = 0;
EIS_NetworkingRequest_t* tr = NULL;
EIS_FacilitiesIndication_t* fi = NULL;
EI1_VCM_t* vcm_rep = calloc(1, sizeof(EI1_VCM_t));
@ -236,25 +238,15 @@ static void vcm_reject(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
mvc_rep->vehicleLength.vehicleLengthConfidenceIndication = 0;
mvc_rep->vehicleWidth = facilities.vehicle.width;
itss_trajectory_lock();
trajectoryA_len = epv.trajectory.len;
memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t));
itss_trajectory_unlock();
trajectoryA[0].latitude = lat;
trajectoryA[0].longitude = lon;
trajectoryA[0].timestamp = now;
++trajectoryA_len;
// Planned trajectory
mvc_rep->plannedTrajectory.list.count = trajectoryA_len - 1;
mvc_rep->plannedTrajectory.list.size = (trajectoryA_len - 1) * sizeof(void*);
mvc_rep->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*));
for (int i = 0; i < trajectoryA_len - 1; ++i) {
mvc_rep->plannedTrajectory.list.count = dpA_len;
mvc_rep->plannedTrajectory.list.size = dpA_len;
mvc_rep->plannedTrajectory.list.array = malloc(dpA_len * sizeof(void*));
for (int i = 0; i < dpA_len; ++i) {
mvc_rep->plannedTrajectory.list.array[i] = calloc(1, sizeof(EI1_STPoint_t));
mvc_rep->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
mvc_rep->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
mvc_rep->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp;
mvc_rep->plannedTrajectory.list.array[i]->deltaLatitude = dpA[i].deltaLatitude._0;
mvc_rep->plannedTrajectory.list.array[i]->deltaLongitude = dpA[i].deltaLongitude._0;
mvc_rep->plannedTrajectory.list.array[i]->deltaTime = dpA[i].deltaTime._0;
}
mvc_rep->negotiation = calloc(1, sizeof(EI1_CoordinationNegotiation_t));
@ -322,16 +314,22 @@ static bool commit() {
vcm_com->header.stationID = facilities.id.station_id;
pthread_mutex_unlock(&facilities.id.lock);
GNSSDeltaPosition dpA[TRAJECTORY_MAX_LEN];
int dpA_len;
int32_t lat, lon;
uint16_t heading;
uint8_t heading_conf;
itss_space_lock();
itss_space_get();
lat = epv.space.latitude;
lon = epv.space.longitude;
heading = epv.space.heading;
heading_conf = epv.space.heading_conf;
lat = epv.space.data.latitude.value;
lon = epv.space.data.longitude.value;
heading = epv.space.data.heading.value;
heading_conf = epv.space.data.heading.confidence;
dpA_len = epv.space.data.ptraj_len;
memcpy(dpA, epv.space.data.ptraj, dpA_len * sizeof(GNSSDeltaPosition));
itss_space_unlock();
vcm_com->vcm.currentPosition.latitude = lat;
vcm_com->vcm.currentPosition.longitude = lon;
@ -348,27 +346,14 @@ static bool commit() {
mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
mvc->vehicleWidth = facilities.vehicle.width;
itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
uint16_t trajectoryA_len = 0;
itss_trajectory_lock();
trajectoryA_len = epv.trajectory.len;
memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t));
itss_trajectory_unlock();
trajectoryA[0].latitude = lat;
trajectoryA[0].longitude = lon;
trajectoryA[0].timestamp = now;
++trajectoryA_len;
mvc->plannedTrajectory.list.count = trajectoryA_len - 1;
mvc->plannedTrajectory.list.size = (trajectoryA_len - 1) * sizeof(void*);
mvc->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*));
for (int i = 0; i < trajectoryA_len - 1; ++i) {
mvc->plannedTrajectory.list.count = dpA_len;
mvc->plannedTrajectory.list.size = dpA_len;
mvc->plannedTrajectory.list.array = malloc(dpA_len * sizeof(void*));
for (int i = 0; i < dpA_len; ++i) {
mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(EI1_STPoint_t));
mvc->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
mvc->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
mvc->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp;
mvc->plannedTrajectory.list.array[i]->deltaLatitude = dpA[i].deltaLatitude._0;
mvc->plannedTrajectory.list.array[i]->deltaLongitude = dpA[i].deltaLongitude._0;
mvc->plannedTrajectory.list.array[i]->deltaTime = dpA[i].deltaTime._0;
}
mvc->negotiation = calloc(1, sizeof(EI1_CoordinationNegotiation_t));
@ -471,12 +456,18 @@ static int vcm_check_handle_request(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
int32_t lat, lon;
uint16_t heading;
uint8_t heading_conf;
itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
ssize_t trajectoryA_len = 0;
GNSSDeltaPosition dpA[TRAJECTORY_MAX_LEN];
int dpA_len;
itss_space_lock();
itss_space_get();
lat = epv.space.latitude;
lon = epv.space.longitude;
heading = epv.space.heading;
heading_conf = epv.space.heading_conf;
lat = epv.space.data.latitude.value;
lon = epv.space.data.longitude.value;
heading = epv.space.data.heading.value;
heading_conf = epv.space.data.heading.confidence;
dpA_len = epv.space.data.ptraj_len;
memcpy(dpA, epv.space.data.ptraj, dpA_len * sizeof(GNSSDeltaPosition));
itss_space_unlock();
double dreq = itss_haversine(
@ -514,9 +505,6 @@ static int vcm_check_handle_request(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
EIS_FacilitiesIndication_t* fi = NULL;
itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
ssize_t trajectoryA_len = 0;
neighbour->proposed = true;
neighbour->session.requested = true;
neighbour->session.nonce = request->nonce;
@ -573,15 +561,15 @@ static int vcm_check_handle_request(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
mvc_rep->vehicleLength.vehicleLengthConfidenceIndication = 0;
mvc_rep->vehicleWidth = facilities.vehicle.width;
itss_trajectory_lock();
trajectoryA_len = epv.trajectory.len;
memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t));
itss_trajectory_unlock();
trajectoryA[0].latitude = lat;
trajectoryA[0].longitude = lon;
trajectoryA[0].timestamp = now;
++trajectoryA_len;
for (int i = 0; i < dpA_len; ++i) {
trajectoryA[i+1].latitude = trajectoryA[i].latitude + dpA[i].deltaLatitude._0;
trajectoryA[i+1].longitude = trajectoryA[i].longitude + dpA[i].deltaLongitude._0;
trajectoryA[i+1].timestamp = trajectoryA[i].timestamp + dpA[i].deltaTime._0;
}
trajectoryA_len = dpA_len + 1;
// Planned trajectory
mvc_rep->plannedTrajectory.list.count = trajectoryA_len - 1;
@ -589,9 +577,9 @@ static int vcm_check_handle_request(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
mvc_rep->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*));
for (int i = 0; i < trajectoryA_len - 1; ++i) {
mvc_rep->plannedTrajectory.list.array[i] = calloc(1, sizeof(EI1_STPoint_t));
mvc_rep->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
mvc_rep->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
mvc_rep->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp;
mvc_rep->plannedTrajectory.list.array[i]->deltaLatitude = dpA[i].deltaLatitude._0;
mvc_rep->plannedTrajectory.list.array[i]->deltaLongitude = dpA[i].deltaLongitude._0;
mvc_rep->plannedTrajectory.list.array[i]->deltaTime = dpA[i].deltaTime._0;
}
// Accepted trajectory
@ -753,30 +741,33 @@ static int intersection_detected(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
uint16_t trajectoryA_len = 0;
GNSSDeltaPosition dpA[TRAJECTORY_MAX_LEN];
int dpA_len;
uint64_t now = itss_time_get();
int32_t lat, lon;
uint16_t heading;
uint8_t heading_conf;
itss_space_lock();
itss_space_get();
lat = epv.space.latitude;
lon = epv.space.longitude;
heading = epv.space.heading;
heading_conf = epv.space.heading_conf;
lat = epv.space.data.latitude.value;
lon = epv.space.data.longitude.value;
heading = epv.space.data.heading.value;
heading_conf = epv.space.data.heading.confidence;
dpA_len = epv.space.data.ptraj_len;
memcpy(dpA, epv.space.data.ptraj, dpA_len * sizeof(GNSSDeltaPosition));
itss_space_unlock();
itss_trajectory_lock();
trajectoryA_len = epv.trajectory.len;
memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t));
itss_trajectory_unlock();
trajectoryA[0].latitude = lat;
trajectoryA[0].longitude = lon;
trajectoryA[0].timestamp = now;
++trajectoryA_len;
for (int i = 0; i < dpA_len; ++i) {
trajectoryA[i+1].latitude = trajectoryA[i].latitude + dpA[i].deltaLatitude._0;
trajectoryA[i+1].longitude = trajectoryA[i].longitude + dpA[i].deltaLongitude._0;
trajectoryA[i+1].timestamp = trajectoryA[i].timestamp + dpA[i].deltaTime._0;
}
trajectoryA_len = dpA_len + 1;
// Initiate conflict resolution
vcm_req = calloc(1, sizeof(EI1_VCM_t));
@ -789,7 +780,7 @@ static int intersection_detected(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
vcm_req->vcm.currentPosition.latitude = lat;
vcm_req->vcm.currentPosition.longitude = lon;
asn_ulong2INTEGER(&vcm_req->vcm.currentPosition.timestamp, now);
asn_uint642INTEGER(&vcm_req->vcm.currentPosition.timestamp, now);
/*
if (coordination->chain.enabled && coordination->chain.id) {
@ -821,14 +812,14 @@ static int intersection_detected(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
mvc->vehicleWidth = facilities.vehicle.width;
// Planned trajectory
mvc->plannedTrajectory.list.count = trajectoryA_len - 1;
mvc->plannedTrajectory.list.size = (trajectoryA_len - 1) * sizeof(void*);
mvc->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*));
for (int i = 0; i < trajectoryA_len - 1; ++i) {
mvc->plannedTrajectory.list.count = dpA_len;
mvc->plannedTrajectory.list.size = dpA_len;
mvc->plannedTrajectory.list.array = malloc(dpA_len * sizeof(void*));
for (int i = 0; i < dpA_len; ++i) {
mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(EI1_STPoint_t));
mvc->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
mvc->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
mvc->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp;
mvc->plannedTrajectory.list.array[i]->deltaLatitude = dpA[i].deltaLatitude._0;
mvc->plannedTrajectory.list.array[i]->deltaLongitude = dpA[i].deltaLongitude._0;
mvc->plannedTrajectory.list.array[i]->deltaTime = dpA[i].deltaTime._0;
}
// Desired trajectory
@ -843,14 +834,14 @@ static int intersection_detected(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
mvc->negotiation->choice.request.nonce = rand();
coordination->session.nonce = mvc->negotiation->choice.request.nonce;
pt->trajectory.list.count = trajectoryA_len - 1;
pt->trajectory.list.size = sizeof(void*) * (trajectoryA_len - 1);
pt->trajectory.list.array = malloc(sizeof(void*) * (trajectoryA_len - 1));
for (int i = 0; i < trajectoryA_len - 1; ++i) {
pt->trajectory.list.count = dpA_len;
pt->trajectory.list.size = dpA_len;
pt->trajectory.list.array = malloc(sizeof(void*) * dpA_len);
for (int i = 0; i < dpA_len; ++i) {
pt->trajectory.list.array[i] = calloc(1, sizeof(EI1_STPoint_t));
pt->trajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
pt->trajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
pt->trajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp;
pt->trajectory.list.array[i]->deltaLatitude = dpA[i].deltaLatitude._0;
pt->trajectory.list.array[i]->deltaLongitude = dpA[i].deltaLongitude._0;
pt->trajectory.list.array[i]->deltaTime = dpA[i].deltaTime._0;
}
pt->offer = 5;
@ -923,25 +914,29 @@ static void intersection_check(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
itss_st_t trajectoryB[TRAJECTORY_MAX_LEN+1]; /* neighbour trajectory */
uint16_t trajectoryA_len = 0;
uint16_t trajectoryB_len = 0;
GNSSDeltaPosition dpA[TRAJECTORY_MAX_LEN];
int dpA_len;
int32_t lat, lon;
itss_space_lock();
itss_space_get();
lat = epv.space.latitude;
lon = epv.space.longitude;
lat = epv.space.data.latitude.value;
lon = epv.space.data.longitude.value;
dpA_len = epv.space.data.ptraj_len;
memcpy(dpA, epv.space.data.ptraj, dpA_len * sizeof(GNSSDeltaPosition));
itss_space_unlock();
uint64_t now = itss_time_get();
itss_trajectory_lock();
trajectoryA_len = epv.trajectory.len;
memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t));
itss_trajectory_unlock();
trajectoryA[0].latitude = lat;
trajectoryA[0].longitude = lon;
trajectoryA[0].timestamp = now;
++trajectoryA_len;
for (int i = 0; i < dpA_len; ++i) {
trajectoryA[i+1].latitude = trajectoryA[i].latitude + dpA[i].deltaLatitude._0;
trajectoryA[i+1].longitude = trajectoryA[i].longitude + dpA[i].deltaLongitude._0;
trajectoryA[i+1].timestamp = trajectoryA[i].timestamp + dpA[i].deltaTime._0;
}
trajectoryA_len = dpA_len + 1;
trajectoryB[0].latitude = vcm->vcm.currentPosition.latitude;
trajectoryB[0].longitude = vcm->vcm.currentPosition.longitude;
@ -1076,8 +1071,6 @@ static int mk_vcm() {
coordination_t* coordination = &facilities.coordination;
itss_st_t trajectory[TRAJECTORY_MAX_LEN];
EI1_VCM_t* vcm = calloc(1, sizeof(EI1_VCM_t));
vcm->header.messageID = 43;
@ -1091,19 +1084,18 @@ static int mk_vcm() {
uint16_t heading;
uint8_t heading_conf;
uint16_t trajectory_len = 0;
GNSSDeltaPosition dpA[TRAJECTORY_MAX_LEN];
int dpA_len;
itss_space_lock();
itss_space_get();
lat = epv.space.latitude;
lon = epv.space.longitude;
heading = epv.space.heading;
heading_conf = epv.space.heading_conf;
lat = epv.space.data.latitude.value;
lon = epv.space.data.longitude.value;
heading = epv.space.data.heading.value;
heading_conf = epv.space.data.heading.confidence;
dpA_len = epv.space.data.ptraj_len;
memcpy(dpA, epv.space.data.ptraj, dpA_len * sizeof(GNSSDeltaPosition));
itss_space_unlock();
itss_trajectory_lock();
trajectory_len = epv.trajectory.len;
memcpy(trajectory, epv.trajectory.path, trajectory_len * sizeof(itss_st_t));
itss_trajectory_unlock();
vcm->vcm.currentPosition.latitude = lat;
vcm->vcm.currentPosition.longitude = lon;
asn_ulong2INTEGER(&vcm->vcm.currentPosition.timestamp, now);
@ -1141,18 +1133,14 @@ static int mk_vcm() {
mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
mvc->vehicleWidth = facilities.vehicle.width;
mvc->plannedTrajectory.list.count = trajectory_len;
mvc->plannedTrajectory.list.size = trajectory_len * sizeof(void*);
mvc->plannedTrajectory.list.array = malloc(trajectory_len * sizeof(void*));
mvc->plannedTrajectory.list.array[0] = calloc(1, sizeof(EI1_STPoint_t));
mvc->plannedTrajectory.list.array[0]->deltaLatitude = trajectory[0].latitude - lat;
mvc->plannedTrajectory.list.array[0]->deltaLongitude = trajectory[0].longitude - lon;
mvc->plannedTrajectory.list.array[0]->deltaTime = trajectory[0].timestamp - now;
for (int i = 1; i < trajectory_len; ++i) {
mvc->plannedTrajectory.list.count = dpA_len;
mvc->plannedTrajectory.list.size = dpA_len * sizeof(void*);
mvc->plannedTrajectory.list.array = malloc(dpA_len * sizeof(void*));
for (int i = 0; i < dpA_len; ++i) {
mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(EI1_STPoint_t));
mvc->plannedTrajectory.list.array[i]->deltaLatitude = trajectory[i].latitude - trajectory[i-1].latitude;
mvc->plannedTrajectory.list.array[i]->deltaLongitude = trajectory[i].longitude - trajectory[i-1].longitude;
mvc->plannedTrajectory.list.array[i]->deltaTime = trajectory[i].timestamp - trajectory[i-1].timestamp;
mvc->plannedTrajectory.list.array[i]->deltaLatitude = dpA[i].deltaLatitude._0;
mvc->plannedTrajectory.list.array[i]->deltaLongitude = dpA[i].deltaLongitude._0;
mvc->plannedTrajectory.list.array[i]->deltaTime = dpA[i].deltaTime._0;
}
}
@ -1178,23 +1166,23 @@ static bool vcm_timer_check(coordination_t* coordination, uint64_t now) {
if (facilities.station_type != EI1_StationType_roadSideUnit) { // Vehicle
itss_trajectory_lock();
itss_space_lock();
if (epv.trajectory.len > 2) {
if (epv.space.data.ptraj_len > 2) {
double c_sum = 0;
for (int i = 0; i < epv.trajectory.len - 2; ++i) {
for (int i = 0; i < epv.space.data.ptraj_len - 2; ++i) {
double d1 = itss_haversine(
epv.trajectory.path[i+1].latitude / 1.0e7,
epv.trajectory.path[i+1].longitude / 1.0e7,
epv.trajectory.path[i].latitude / 1.0e7,
epv.trajectory.path[i].longitude / 1.0e7);
epv.space.data.ptraj[i].deltaLatitude._0 / 1.0e7,
epv.space.data.ptraj[i].deltaLongitude._0 / 1.0e7,
0.0,
0.0);
double t1 = atan2(
epv.trajectory.path[i+1].latitude - epv.trajectory.path[i].latitude ,
epv.trajectory.path[i+1].longitude - epv.trajectory.path[i].longitude);
epv.space.data.ptraj[i].deltaLatitude._0,
epv.space.data.ptraj[i].deltaLongitude._0);
double t2 = atan2(
epv.trajectory.path[i+2].latitude - epv.trajectory.path[i].latitude ,
epv.trajectory.path[i+2].longitude - epv.trajectory.path[i].longitude);
epv.space.data.ptraj[i+1].deltaLatitude._0 + epv.space.data.ptraj[i].deltaLatitude._0,
epv.space.data.ptraj[i+1].deltaLongitude._0 + epv.space.data.ptraj[i].deltaLongitude._0);
c_sum += fabs(sin(t2-t1)*d1);
}
@ -1205,7 +1193,7 @@ static bool vcm_timer_check(coordination_t* coordination, uint64_t now) {
}
}
itss_trajectory_unlock(epv);
itss_space_unlock(epv);
}
return send;

8
src/vcm.h
View File

@ -12,6 +12,7 @@
#define MC_AFF_STATIONS_N_MAX 7
#define MC_HASH_LINK_LEN 32
#define MC_SESSIONS_N_MAX 16
#define TRAJECTORY_MAX_LEN 16
typedef enum MC_PROTOCOL {
MC_PROTOCOL_VCM_RR,
@ -19,6 +20,13 @@ typedef enum MC_PROTOCOL {
MC_PROTOCOL_VCM_RRAC
} MC_PROTOCOL_e;
typedef struct itss_st {
int32_t latitude;
int32_t longitude;
int32_t altitude;
uint64_t timestamp;
} itss_st_t;
typedef struct mc_neighbour {
uint32_t station_id;
uint16_t length;