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

Delta ASN VCM trjs

This commit is contained in:
emanuel 2022-10-19 12:28:46 +01:00
parent fb7102a933
commit 27df6655c5
1 changed files with 21 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
src/vcm.c
View File

@ -328,14 +328,14 @@ static int vcm_check_intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour
mvc->plannedTrajectory.list.size = trajectoryA_len * sizeof(void*);
mvc->plannedTrajectory.list.array = malloc(trajectoryA_len * sizeof(void*));
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;
asn_ulong2INTEGER(&mvc->plannedTrajectory.list.array[0]->timestamp, trajectoryA[0].timestamp - now);
mvc->plannedTrajectory.list.array[0]->deltaLatitude = trajectoryA[0].latitude - lat;
mvc->plannedTrajectory.list.array[0]->deltaLongitude = trajectoryA[0].longitude - lon;
mvc->plannedTrajectory.list.array[0]->deltaTime = 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].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);
mvc->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i].latitude - trajectoryA[i-1].latitude;
mvc->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i].longitude - trajectoryA[i-1].longitude;
mvc->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i].timestamp - trajectoryA[i-1].timestamp;
}
// Desired trajectory
@ -353,14 +353,14 @@ static int vcm_check_intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour
pt->trajectory.list.size = sizeof(void*) * trajectoryA_len;
pt->trajectory.list.array = malloc(sizeof(void*) * trajectoryA_len);
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;
asn_ulong2INTEGER(&pt->trajectory.list.array[0]->timestamp, trajectoryA[0].timestamp - now);
pt->trajectory.list.array[0]->deltaLatitude = trajectoryA[0].latitude - lat;
pt->trajectory.list.array[0]->deltaLongitude = trajectoryA[0].longitude - lon;
pt->trajectory.list.array[0]->deltaTime = 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].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->trajectory.list.array[i]->deltaLatitude = trajectoryA[i].latitude - trajectoryA[i-1].latitude;
pt->trajectory.list.array[i]->deltaLongitude = trajectoryA[i].longitude - trajectoryA[i-1].longitude;
pt->trajectory.list.array[i]->deltaTime = trajectoryA[i].timestamp - trajectoryA[i-1].timestamp;
}
pt->offer = 5;
@ -517,10 +517,9 @@ int vcm_check(VCM_t* vcm) {
asn_INTEGER2ulong(&vcm->vcm.currentPosition.timestamp, (unsigned long long*) &trajectoryB[0].timestamp);
++trajectoryB_len;
for (int i = 1; i < mvc->plannedTrajectory.list.count && i < TRAJECTORY_MAX_LEN; ++i) {
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[i].latitude = mvc->plannedTrajectory.list.array[i]->deltaLatitude + trajectoryB[i-1].latitude;
trajectoryB[i].longitude = mvc->plannedTrajectory.list.array[i]->deltaLongitude + trajectoryB[i-1].longitude;
trajectoryB[i].timestamp = mvc->plannedTrajectory.list.array[i]->deltaTime + trajectoryB[i-1].timestamp;
++trajectoryB_len;
}
@ -615,14 +614,14 @@ static int mk_vcm(uint8_t* vcm_uper, uint16_t* vcm_uper_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(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);
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.array[i] = calloc(1, sizeof(STPoint_t));
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);
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;
}
}