AP_IRLock: add SITL IRLock

AP_IRLock: simplify include

AP_IRLock: reduce scope of gazebo irlock_paquet

AP_IRLock: add z pos for IRLock SITL, set to 1.0 for pixycam

AP_IRLock: remove unused target size and get_angle_to_target_rad function

libraries/AP_IRLock/AP_IRLock.h

@@ -12,5 +12,6 @@
 #include "AP_IRLock_I2C.h"
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include "AP_IRLock_SITL_Gazebo.h"
 #include "AP_IRLock_SITL.h"
 #endif

libraries/AP_IRLock/AP_IRLock_I2C.cpp

@@ -25,6 +25,7 @@
 #include <stdio.h>
 #include <utility>
 #include <AP_HAL/I2CDevice.h>
+#include <AP_Common/Semaphore.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -137,8 +138,7 @@ void AP_IRLock_I2C::read_frames(void)
         _target_info.timestamp = AP_HAL::millis();
         _target_info.pos_x = 0.5f*(corner1_pos_x+corner2_pos_x);
         _target_info.pos_y = 0.5f*(corner1_pos_y+corner2_pos_y);
-        _target_info.size_x = corner2_pos_x-corner1_pos_x;
-        _target_info.size_y = corner2_pos_y-corner1_pos_y;
+        _target_info.pos_z = 1.0f;
     }
 
 #if 0
@@ -148,7 +148,7 @@ void AP_IRLock_I2C::read_frames(void)
         lastt = _target_info.timestamp;
         printf("pos_x:%.5f pos_y:%.5f size_x:%.6f size_y:%.5f\n", 
                _target_info.pos_x, _target_info.pos_y,
-               _target_info.size_x, _target_info.size_y);
+               (corner2_pos_x-corner1_pos_x), (corner2_pos_y-corner1_pos_y));
     }
 #endif
 }

libraries/AP_IRLock/AP_IRLock_I2C.h

@@ -5,6 +5,7 @@
 #pragma once
 
 #include "IRLock.h"
+#include <AP_HAL/AP_HAL.h>
 
 class AP_IRLock_I2C : public IRLock
 {

libraries/AP_IRLock/AP_IRLock_SITL.cpp

@@ -23,64 +23,36 @@
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 #include "AP_IRLock_SITL.h"
-#include <SITL/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)
-{}
+#include "AP_AHRS/AP_AHRS.h"
 
 void AP_IRLock_SITL::init(int8_t bus)
 {
-    SITL::SITL *sitl = AP::sitl();
-    // 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", sitl->irlock_port);
-
-    sock.reuseaddress();
-    sock.set_blocking(false);
-
-    hal.console->printf("AP_IRLock_SITL::init()\n");
-
-    _flags.healthy = true;
+    _sitl = AP::sitl();
+    _sitl->precland_sim._type.set_and_notify(SITL::SIM_Precland::PreclandType::PRECLAND_TYPE_CONE);
 }
 
 // retrieve latest sensor data - returns true if new data is available
 bool AP_IRLock_SITL::update()
 {
     // return immediately if not healthy
+    _flags.healthy = _sitl->precland_sim.healthy();
     if (!_flags.healthy) {
         return false;
     }
 
-    // receive packet from Gazebo IRLock plugin
-    irlock_packet 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) {
-        // 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;
-        _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;
+    if (_sitl->precland_sim.last_update_ms() != _last_timestamp) {
+        const Vector3f position = _sitl->precland_sim.get_target_position();
+        const Matrix3f &body_to_ned = AP::ahrs().get_rotation_body_to_ned();
+        const Vector3f real_position =  body_to_ned.mul_transpose(-position);
+        _last_timestamp = _sitl->precland_sim.last_update_ms();
         _last_update_ms = _last_timestamp;
-        new_data = true;
+        _target_info.timestamp = _last_timestamp;
+        _target_info.pos_x = real_position.y;
+        _target_info.pos_y = -real_position.x;
+        _target_info.pos_z = real_position.z;
+        return true;
     }
-
-    // return true if new data found
-    return new_data;
+    return false;
 }
 
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL

libraries/AP_IRLock/AP_IRLock_SITL.h

@@ -6,15 +6,13 @@
  */
 #pragma once
 
-#include <AP_HAL/utility/Socket.h>
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 #include "IRLock.h"
+#include <SITL/SITL.h>
 
 class AP_IRLock_SITL : public IRLock
 {
 public:
-    AP_IRLock_SITL();
-
     // init - initialize sensor library
     void init(int8_t bus) override;
 
@@ -22,20 +20,7 @@ public:
     bool update() override;
 
 private:
-
-    /*
-      reply packet sent from simulator to ArduPilot
-     */
-    struct irlock_packet {
-        uint64_t timestamp;  // in miliseconds
-        uint16_t num_targets;
-        float pos_x;
-        float pos_y;
-        float size_x;
-        float size_y;
-    };
-
-    uint32_t _last_timestamp;
-    SocketAPM sock;
+    SITL::SITL          *_sitl;                 // sitl instance pointer
+    uint32_t _last_timestamp = 0;
 };
 #endif // CONFIG_HAL_BOARD

libraries/AP_IRLock/AP_IRLock_SITL_Gazebo.cpp

@@ -0,0 +1,95 @@
+/*
+   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_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();
+    // 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", sitl->irlock_port);
+
+    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_Gazebo::update()
+{
+    // return immediately if not healthy
+    if (!_flags.healthy) {
+        return false;
+    }
+
+    // receive packet from Gazebo IRLock plugin
+
+    /*
+      reply packet sent from simulator to ArduPilot
+     */
+    struct irlock_packet {
+        uint64_t timestamp;  // in miliseconds
+        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) {
+        // 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;
+        _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 true if new data found
+    return new_data;
+}
+
+#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL

libraries/AP_IRLock/AP_IRLock_SITL_Gazebo.h

@@ -0,0 +1,29 @@
+/*
+ * AP_IRLock_SITL.h
+ *
+ *  Created on: June 09, 2016
+ *      Author: Ian Chen
+ */
+#pragma once
+
+#include <AP_HAL/utility/Socket.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include "IRLock.h"
+
+class AP_IRLock_SITL_Gazebo : public IRLock
+{
+public:
+    AP_IRLock_SITL_Gazebo();
+
+    // init - initialize sensor library
+    void init(int8_t bus) override;
+
+    // retrieve latest sensor data - returns true if new data is available
+    bool update() override;
+
+private:
+
+    uint32_t _last_timestamp;
+    SocketAPM sock;
+};
+#endif // CONFIG_HAL_BOARD

libraries/AP_IRLock/IRLock.cpp

@@ -7,21 +7,6 @@
 
 #include "IRLock.h"
 
-// retrieve body frame x and y angles (in radians) to target
-// returns true if data is available
-bool IRLock::get_angle_to_target_rad(float &x_angle_rad, float &y_angle_rad) const
-{
-    // return false if we have no target
-    if (!_flags.healthy) {
-        return false;
-    }
-
-    // use data from first (largest) object
-    x_angle_rad = atanf(_target_info.pos_x);
-    y_angle_rad = atanf(_target_info.pos_y);
-    return true;
-}
-
 // retrieve body frame unit vector in direction of target
 // returns true if data is available
 bool IRLock::get_unit_vector_body(Vector3f& ret) const
@@ -34,7 +19,7 @@ bool IRLock::get_unit_vector_body(Vector3f& ret) const
     // use data from first (largest) object
     ret.x = -_target_info.pos_y;
     ret.y = _target_info.pos_x;
-    ret.z = 1.0f;
+    ret.z = _target_info.pos_z;
     ret /= ret.length();
     return true;
 }

libraries/AP_IRLock/IRLock.h

@@ -21,7 +21,7 @@
  */
 #pragma once
 
-#include <AP_AHRS/AP_AHRS.h>
+#include <AP_Math/AP_Math.h>
 
 class IRLock
 {
@@ -42,10 +42,6 @@ public:
     // retrieve latest sensor data - returns true if new data is available
     virtual bool update() = 0;
 
-    // retrieve body frame x and y angles (in radians) to target
-    // returns true if data is available
-    bool get_angle_to_target_rad(float &x_angle_rad, float &y_angle_rad) const;
-    
     // retrieve body frame unit vector in direction of target
     // returns true if data is available
     bool get_unit_vector_body(Vector3f& ret) const;
@@ -64,8 +60,7 @@ protected:
         uint32_t timestamp;   // milliseconds since system start
         float pos_x;          // x-axis distance from center of image to center of target in units of tan(theta)
         float pos_y;          // y-axis distance from center of image to center of target in units of tan(theta)
-        float size_x;         // size of target along x-axis in units of tan(theta)
-        float size_y;         // size of target along y-axis in units of tan(theta)
+        float pos_z;
     } irlock_target_info;
 
     irlock_target_info _target_info;