Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_IRLock_SITL: correct packet timestamp

This commit is contained in:
Pierre Kancir 2017-01-19 15:17:08 +01:00 committed by Francisco Ferreira
parent fd0e4164b9
commit 83c7217911
2 changed files with 6 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_IRLock/AP_IRLock_SITL.cpp
View File

@ -62,19 +62,18 @@ bool AP_IRLock_SITL::update()
// receive packet from Gazebo IRLock plugin // receive packet from Gazebo IRLock plugin
irlock_packet pkt; irlock_packet pkt;
const int wait_ms = 0; 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; bool new_data = false;
// fprintf(stderr, "sitl %d %d\n", i, _num_targets); if (s == sizeof(irlock_packet) && pkt.timestamp > _last_timestamp) {
if (s == sizeof(irlock_packet) && pkt.timestamp/1000 > _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); // 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_x = pkt.pos_x;
_target_info.pos_y = pkt.pos_y; _target_info.pos_y = pkt.pos_y;
_target_info.size_x = pkt.size_x; _target_info.size_x = pkt.size_x;
_target_info.size_y = pkt.size_y; _target_info.size_y = pkt.size_y;
_last_timestamp = pkt.timestamp/1000; _last_timestamp = pkt.timestamp;
_last_update_ms = _last_timestamp; _last_update_ms = _last_timestamp;
new_data = true; new_data = true;
} }

2
libraries/AP_IRLock/AP_IRLock_SITL.h
View File

@ -27,7 +27,7 @@ private:
reply packet sent from simulator to ArduPilot reply packet sent from simulator to ArduPilot
*/ */
struct irlock_packet { struct irlock_packet {
uint64_t timestamp; uint64_t timestamp; // in miliseconds
uint16_t num_targets; uint16_t num_targets;
float pos_x; float pos_x;
float pos_y; float pos_y;