1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495 |
- #include <AP_HAL/AP_HAL.h>
- #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
- #include "AP_IRLock_SITL_Gazebo.h"
- #include <SITL/SITL.h>
- #include <fcntl.h>
- #include <unistd.h>
- #include <iostream>
- extern const AP_HAL::HAL& hal;
- AP_IRLock_SITL_Gazebo::AP_IRLock_SITL_Gazebo() :
- _last_timestamp(0),
- sock(true)
- {}
- void AP_IRLock_SITL_Gazebo::init(int8_t bus)
- {
- SITL::SITL *sitl = AP::sitl();
-
-
-
- sock.bind("127.0.0.1", sitl->irlock_port);
- sock.reuseaddress();
- sock.set_blocking(false);
- hal.console->printf("AP_IRLock_SITL::init()\n");
- _flags.healthy = true;
- }
- bool AP_IRLock_SITL_Gazebo::update()
- {
-
- if (!_flags.healthy) {
- return false;
- }
-
-
- struct irlock_packet {
- uint64_t timestamp;
- uint16_t num_targets;
- float pos_x;
- float pos_y;
- float size_x;
- float size_y;
- } pkt;
- const int wait_ms = 0;
- ssize_t s = sock.recv(&pkt, sizeof(irlock_packet), wait_ms);
- bool new_data = false;
- if (s == sizeof(irlock_packet) && pkt.timestamp > _last_timestamp) {
-
- _target_info.timestamp = pkt.timestamp;
- _target_info.pos_x = pkt.pos_x;
- _target_info.pos_y = pkt.pos_y;
- _last_timestamp = pkt.timestamp;
- _last_update_ms = _last_timestamp;
- new_data = true;
- }
-
- return new_data;
- }
- #endif
|