SITL: implement Gazebo SITL for IRLock tracking.

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -67,7 +67,7 @@ void AC_PrecLand::init()
             _backend = new AC_PrecLand_Companion(*this, _backend_state);
             break;
         // IR Lock
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_SITL
         case PRECLAND_TYPE_IRLOCK:
             _backend = new AC_PrecLand_IRLock(*this, _backend_state);
             break;

libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp

@@ -5,7 +5,7 @@
 extern const AP_HAL::HAL& hal;
 
 // this only builds for PX4 so far
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_SITL
 
 // Constructor
 AC_PrecLand_IRLock::AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state)

libraries/AC_PrecLand/AC_PrecLand_IRLock.h

@@ -6,8 +6,12 @@
 #include <AC_PrecLand/AC_PrecLand_Backend.h>
 #include <AP_IRLock/AP_IRLock.h>
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include <AP_IRLock/AP_IRLock_SITL.h>
+#endif
+
 // this only builds for PX4 so far
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_SITL
 
 /*
  * AC_PrecLand_IRLock - implements precision landing using target vectors provided
@@ -41,8 +45,12 @@ public:
     void handle_msg(mavlink_message_t* msg) {};
 
 private:
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    AP_IRLock_SITL irlock;
+#else
     AP_IRLock_PX4 irlock;
-    
+#endif
+
     Vector3f            _los_meas_body;         // unit vector in body frame pointing towards target
     bool                _have_los_meas;         // true if there is a valid measurement from the camera
     uint32_t            _los_meas_time_ms;      // system time in milliseconds when los was measured

libraries/AP_IRLock/AP_IRLock.h

@@ -10,3 +10,7 @@
 
 #include "IRLock.h"
 #include "AP_IRLock_PX4.h"
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include "AP_IRLock_SITL.h"
+#endif

libraries/AP_IRLock/AP_IRLock_SITL.cpp

@@ -0,0 +1,91 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ * AP_IRLock_SITL.cpp
+ *
+ *  Created on: June 09, 2016
+ *      Author: Ian Chen
+ */
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include "AP_IRLock_SITL.h"
+
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <iostream>
+
+extern const AP_HAL::HAL& hal;
+
+AP_IRLock_SITL::AP_IRLock_SITL() :
+    _last_timestamp(0),
+    sock(true)
+{}
+
+void AP_IRLock_SITL::init()
+{
+    // try to bind to a specific port so that if we restart ArduPilot
+    // Gazebo keeps sending us packets. Not strictly necessary but
+    // useful for debugging
+    sock.bind("127.0.0.1", 9005);
+
+    sock.reuseaddress();
+    sock.set_blocking(false);
+
+    hal.console->printf("AP_IRLock_SITL::init()\n");
+
+    _flags.healthy = true;
+}
+
+// retrieve latest sensor data - returns true if new data is available
+bool AP_IRLock_SITL::update()
+{
+    // return immediately if not healthy
+    if (!_flags.healthy) {
+        return false;
+    }
+
+    // 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);
+
+    // TODO: need to fix this
+    // for some reason, pkt.num_targets occasionally gets a bad number.
+    // hardcode _num_targets to 1 for now.
+    // _num_targets = pkt.num_targets;
+    _num_targets = 1;
+
+    for (uint16_t i=0; i<_num_targets; ++i) {
+      // fprintf(stderr, "sitl %d %d\n", i, _num_targets);
+      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[i].timestamp = pkt.timestamp;
+          _target_info[i].pos_x = pkt.pos_x;
+          _target_info[i].pos_y = pkt.pos_y;
+          _target_info[i].size_x = pkt.size_x;
+          _target_info[i].size_y = pkt.size_y;
+          _last_timestamp = pkt.timestamp;
+          _last_update_ms = AP_HAL::millis();
+      }
+    }
+
+    // return true if new data found
+    return (_num_targets > 0);
+}
+
+#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL

libraries/AP_IRLock/AP_IRLock_SITL.h

@@ -0,0 +1,40 @@
+/*
+ * AP_IRLock_SITL.h
+ *
+ *  Created on: June 09, 2016
+ *      Author: Ian Chen
+ */
+#pragma once
+
+#include <AP_HAL/utility/Socket.h>
+
+#include "IRLock.h"
+
+class AP_IRLock_SITL : public IRLock
+{
+public:
+    AP_IRLock_SITL();
+
+    // init - initialize sensor library
+    virtual void init();
+
+    // retrieve latest sensor data - returns true if new data is available
+    virtual bool update();
+
+private:
+
+    /*
+      reply packet sent from simulator to ArduPilot
+     */
+    struct irlock_packet {
+            uint64_t timestamp;
+            uint16_t num_targets;
+            float pos_x;
+            float pos_y;
+            float size_x;
+            float size_y;
+          };
+
+    uint64_t _last_timestamp;
+    SocketAPM sock;
+};