Spiri/ROSBuzz_MISTLab
13
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

debug serialization

Browse Source
This commit is contained in:
vivek-shankar 2018-07-29 00:21:35 -04:00
parent efb43f0543
commit 522e07f5a3
1 changed files with 13 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
src/roscontroller.cpp
View File

@ -610,15 +610,17 @@ with size......... | /
msg_id++; msg_id++;
payload_out.sysid = (uint8_t)robot_id; payload_out.sysid = (uint8_t)robot_id;
payload_out.msgid = (uint32_t)msg_id; payload_out.msgid = (uint32_t)msg_id;
if(buzz_utility::get_timesync_state()){ uint8_t mtype;
// prepare rosbuzz msg header
uint8_t mtype = (uint8_t)BUZZ_MESSAGE_TIME;
uint16_t mid = (uint16_t)msg_id; uint16_t mid = (uint16_t)msg_id;
get_logical_time(); get_logical_time();
uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; uint32_t stime = (uint32_t)logical_clock.toSec() * 100000;
ROS_INFO("Sent stime %f for %d",stime,mid); ROS_INFO("Sent stime %u for %u",stime,mid);
uint8_t r8header[8]; uint8_t r8header[8];
uint64_t rheader = 0; uint64_t rheader = 0;
if(buzz_utility::get_timesync_state()){
// prepare rosbuzz msg header
mtype = (uint8_t)BUZZ_MESSAGE_TIME;
memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header, &mtype, sizeof(uint8_t));
memcpy(r8header+1, &mid, sizeof(uint16_t)); memcpy(r8header+1, &mid, sizeof(uint16_t));
memcpy(r8header+3, &stime, sizeof(uint32_t)); memcpy(r8header+3, &stime, sizeof(uint32_t));
@ -635,13 +637,7 @@ with size......... | /
} }
else{ else{
// prepare rosbuzz msg header // prepare rosbuzz msg header
uint8_t mtype = (uint8_t)BUZZ_MESSAGE_WTO_TIME; mtype = (uint8_t)BUZZ_MESSAGE_WTO_TIME;
uint16_t mid = (uint16_t)msg_id;
get_logical_time();
uint32_t stime = (uint32_t)logical_clock.toSec() * 100000;
ROS_INFO("Sent stime %f for %d",stime,mid);
uint8_t r8header[8];
uint64_t rheader = 0;
memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header, &mtype, sizeof(uint8_t));
memcpy(r8header+1, &mid, sizeof(uint16_t)); memcpy(r8header+1, &mid, sizeof(uint16_t));
memcpy(r8header+3, &stime, sizeof(uint32_t)); memcpy(r8header+3, &stime, sizeof(uint32_t));
@ -696,12 +692,10 @@ with size......... | /
memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t)); memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t));
free(buff_send); free(buff_send);
// prepare rosbuzz msg header // prepare rosbuzz msg header
uint8_t mtype = (uint8_t)UPDATER_MESSAGE; mtype = (uint8_t)UPDATER_MESSAGE;
uint16_t mid = (uint16_t)msg_id; mid = (uint16_t)msg_id;
get_logical_time(); get_logical_time();
uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; stime = (uint32_t)logical_clock.toSec() * 100000;
uint8_t r8header[8];
uint64_t rheader = 0;
memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header, &mtype, sizeof(uint8_t));
memcpy(r8header+1, &mid, sizeof(uint16_t)); memcpy(r8header+1, &mid, sizeof(uint16_t));
memcpy(r8header+3, &stime, sizeof(uint32_t)); memcpy(r8header+3, &stime, sizeof(uint32_t));