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

VCM: fix planned trajectory

This commit is contained in:
emanuel 2022-10-19 10:02:53 +01:00
parent e92fba0a32
commit c45ac7c2e4
1 changed files with 20 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/vcm.c
View File

@ -234,7 +234,6 @@ static int vcm_check_handle_reply(VCM_t* vcm, mc_neighbour_s* neighbour) {
log_info("[vc] received VCM.reply is response to another ITS-S VCM.request");
}
neighbour->intersecting = false;
neighbour->proposed = false;
@ -331,11 +330,12 @@ static int vcm_check_intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour
mvc->plannedTrajectory.list.array[0] = calloc(1, sizeof(STPoint_t));
mvc->plannedTrajectory.list.array[0]->latitude = trajectoryA[0].latitude - lat;
mvc->plannedTrajectory.list.array[0]->longitude = trajectoryA[0].longitude - lon;
for (int i = 0; i < trajectoryA_len - 1; ++i) {
asn_ulong2INTEGER(&mvc->plannedTrajectory.list.array[0]->timestamp, trajectoryA[0].timestamp - now);
for (int i = 1; i < trajectoryA_len; ++i) {
mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
mvc->plannedTrajectory.list.array[i]->latitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
mvc->plannedTrajectory.list.array[i]->longitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
asn_ulong2INTEGER(&mvc->plannedTrajectory.list.array[i]->timestamp, trajectoryA[i].timestamp);
mvc->plannedTrajectory.list.array[i]->latitude = trajectoryA[i].latitude - trajectoryA[i-1].latitude;
mvc->plannedTrajectory.list.array[i]->longitude = trajectoryA[i].longitude - trajectoryA[i-1].longitude;
asn_ulong2INTEGER(&mvc->plannedTrajectory.list.array[i]->timestamp, trajectoryA[i].timestamp - trajectoryA[i-1].timestamp);
}
// Desired trajectory
@ -355,11 +355,12 @@ static int vcm_check_intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour
pt->trajectory.list.array[0] = calloc(1, sizeof(STPoint_t));
pt->trajectory.list.array[0]->latitude = trajectoryA[0].latitude - lat;
pt->trajectory.list.array[0]->longitude = trajectoryA[0].longitude - lon;
for (int i = 0; i < trajectoryA_len - 1; ++i) {
asn_ulong2INTEGER(&pt->trajectory.list.array[0]->timestamp, trajectoryA[0].timestamp - now);
for (int i = 1; i < trajectoryA_len; ++i) {
pt->trajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
pt->trajectory.list.array[i]->latitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
pt->trajectory.list.array[i]->longitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
asn_ulong2INTEGER(&pt->trajectory.list.array[i]->timestamp, trajectoryA[i].timestamp);
pt->trajectory.list.array[i]->latitude = trajectoryA[i].latitude - trajectoryA[i-1].latitude;
pt->trajectory.list.array[i]->longitude = trajectoryA[i].longitude - trajectoryA[i-1].longitude;
asn_ulong2INTEGER(&pt->trajectory.list.array[i]->timestamp, trajectoryA[i].timestamp - trajectoryA[i-1].timestamp);
}
pt->offer = 5;
@ -484,7 +485,6 @@ int vcm_check(VCM_t* vcm) {
ni = coordination->neighbours_len;
++coordination->neighbours_len;
}
switch (vcm->vcm.maneuverContainer.present) {
case ManeuverContainer_PR_vehicle:
;
@ -520,6 +520,7 @@ int vcm_check(VCM_t* vcm) {
trajectoryB[i].latitude = mvc->plannedTrajectory.list.array[i]->latitude + trajectoryB[i-1].latitude;
trajectoryB[i].longitude = mvc->plannedTrajectory.list.array[i]->longitude + trajectoryB[i-1].longitude;
asn_INTEGER2ulong(&mvc->plannedTrajectory.list.array[i]->timestamp, (unsigned long long*) &trajectoryB[i].timestamp);
trajectoryB[i].timestamp += trajectoryB[i-1].timestamp;
++trajectoryB_len;
}
@ -613,11 +614,15 @@ static int mk_vcm(uint8_t* vcm_uper, uint16_t* vcm_uper_len) {
mvc->plannedTrajectory.list.count = trajectory_len;
mvc->plannedTrajectory.list.size = trajectory_len * sizeof(void*);
mvc->plannedTrajectory.list.array = malloc(trajectory_len * sizeof(void*));
for (int i = 0; i < trajectory_len; ++i) {
mvc->plannedTrajectory.list.array[0] = calloc(1, sizeof(STPoint_t));
mvc->plannedTrajectory.list.array[0]->latitude = trajectory[0].latitude - lat;
mvc->plannedTrajectory.list.array[0]->longitude = trajectory[0].longitude - lon;
asn_ulong2INTEGER(&mvc->plannedTrajectory.list.array[0]->timestamp, trajectory[0].timestamp - now);
for (int i = 1; i < trajectory_len; ++i) {
mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
mvc->plannedTrajectory.list.array[i]->latitude = trajectory[i].latitude;
mvc->plannedTrajectory.list.array[i]->longitude = trajectory[i].longitude;
asn_ulong2INTEGER(&mvc->plannedTrajectory.list.array[i]->timestamp, trajectory[i].timestamp);
mvc->plannedTrajectory.list.array[i]->latitude = trajectory[i].latitude - trajectory[i-1].latitude;
mvc->plannedTrajectory.list.array[i]->longitude = trajectory[i].longitude - trajectory[i-1].longitude;
asn_ulong2INTEGER(&mvc->plannedTrajectory.list.array[i]->timestamp, trajectory[i].timestamp - trajectory[i-1].timestamp);
}
}
@ -686,6 +691,7 @@ void* vc_service() {
if (vcm_timer_check(coordination, now)) {
rv = mk_vcm(bpr->data.buf, (uint16_t *) &bpr->data.size);
if (rv) {
pthread_mutex_unlock(&coordination->lock);
continue;
}