TiagoRG
/
AutowareV2X
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Log age of cpm (difference between gdt)

This commit is contained in:
Yu Asabe 2021-12-05 15:22:00 +09:00
parent cc7ac3ce44
commit 678a29efc7
1 changed files with 7 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/cpm_application.cpp
View File

@ -79,6 +79,13 @@ namespace v2x
asn1::Cpm message = *cpm; asn1::Cpm message = *cpm;
ItsPduHeader_t &header = message->header; 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; CpmManagementContainer_t &management = message->cpm.cpmParameters.managementContainer;
double lat = management.referencePosition.latitude / 1.0e7; double lat = management.referencePosition.latitude / 1.0e7;
double lon = management.referencePosition.longitude / 1.0e7; double lon = management.referencePosition.longitude / 1.0e7;