From 678a29efc7e0ac5a21fc37c2e299c316ab13a42e Mon Sep 17 00:00:00 2001
From: Yu Asabe <yuasabe@g.ecc.u-tokyo.ac.jp>
Date: Sun, 5 Dec 2021 15:22:00 +0900
Subject: [PATCH] Log age of cpm (difference between gdt)

---
 src/cpm_application.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp
index 5b65e12..0dac301 100644
--- a/src/cpm_application.cpp
+++ b/src/cpm_application.cpp
@@ -79,6 +79,13 @@ namespace v2x
       asn1::Cpm message = *cpm;
       ItsPduHeader_t &header = message->header;
 
+      // Calculate GDT and get GDT from CPM and calculate the "Age of CPM"
+      GenerationDeltaTime_t gdt_cpm = message->cpm.generationDeltaTime;
+      const auto time_now = duration_cast<milliseconds> (runtime_.now().time_since_epoch());
+      uint16_t gdt = time_now.count();
+      int gdt_diff = (65536 + (gdt - gdt_cpm) % 65536) % 65536;
+      RCLCPP_INFO(node_->get_logger(), "gdt: %ld %u %d", gdt_cpm, gdt, gdt_diff);
+
       CpmManagementContainer_t &management = message->cpm.cpmParameters.managementContainer;
       double lat = management.referencePosition.latitude / 1.0e7;
       double lon = management.referencePosition.longitude / 1.0e7;