diff --git a/src/vcm.c b/src/vcm.c
index 4660b70..9100d84 100644
--- a/src/vcm.c
+++ b/src/vcm.c
@@ -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;
             }