From 83c7217911e2df822f28161e9e3b759cf67bb496 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 19 Jan 2017 15:17:08 +0100 Subject: [PATCH] AP_IRLock_SITL: correct packet timestamp --- libraries/AP_IRLock/AP_IRLock_SITL.cpp | 11 +++++------ libraries/AP_IRLock/AP_IRLock_SITL.h | 2 +- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/libraries/AP_IRLock/AP_IRLock_SITL.cpp b/libraries/AP_IRLock/AP_IRLock_SITL.cpp index 3ff5b28ac4..f02d52d6ef 100644 --- a/libraries/AP_IRLock/AP_IRLock_SITL.cpp +++ b/libraries/AP_IRLock/AP_IRLock_SITL.cpp @@ -62,19 +62,18 @@ bool AP_IRLock_SITL::update() // receive packet from Gazebo IRLock plugin irlock_packet pkt; const int wait_ms = 0; - size_t s = sock.recv(&pkt, sizeof(irlock_packet), wait_ms); + ssize_t s = sock.recv(&pkt, sizeof(irlock_packet), wait_ms); bool new_data = false; - - // fprintf(stderr, "sitl %d %d\n", i, _num_targets); - if (s == sizeof(irlock_packet) && pkt.timestamp/1000 > _last_timestamp) { + + if (s == sizeof(irlock_packet) && pkt.timestamp > _last_timestamp) { // fprintf(stderr, " posx %f posy %f sizex %f sizey %f\n", pkt.pos_x, pkt.pos_y, pkt.size_x, pkt.size_y); - _target_info.timestamp = pkt.timestamp / 1000; + _target_info.timestamp = pkt.timestamp; _target_info.pos_x = pkt.pos_x; _target_info.pos_y = pkt.pos_y; _target_info.size_x = pkt.size_x; _target_info.size_y = pkt.size_y; - _last_timestamp = pkt.timestamp/1000; + _last_timestamp = pkt.timestamp; _last_update_ms = _last_timestamp; new_data = true; } diff --git a/libraries/AP_IRLock/AP_IRLock_SITL.h b/libraries/AP_IRLock/AP_IRLock_SITL.h index 2d7a8aa00e..2f5d5cd4bc 100644 --- a/libraries/AP_IRLock/AP_IRLock_SITL.h +++ b/libraries/AP_IRLock/AP_IRLock_SITL.h @@ -27,7 +27,7 @@ private: reply packet sent from simulator to ArduPilot */ struct irlock_packet { - uint64_t timestamp; + uint64_t timestamp; // in miliseconds uint16_t num_targets; float pos_x; float pos_y;