diff --git a/src/vcm.c b/src/vcm.c index e080912..d1cfc9e 100644 --- a/src/vcm.c +++ b/src/vcm.c @@ -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; } }