AC_PrecLand: make SITL_Gazebo an alternate backend

Preprocessor directives were used to hack in the use
of AP_IRLock_SITL.  Instead, make it a full backend itself.

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -3,6 +3,7 @@
 #include "AC_PrecLand_Backend.h"
 #include "AC_PrecLand_Companion.h"
 #include "AC_PrecLand_IRLock.h"
+#include "AC_PrecLand_SITL_Gazebo.h"
 
 extern const AP_HAL::HAL& hal;
 
@@ -17,7 +18,7 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Precision Land Type
     // @Description: Precision Land Type
-    // @Values: 0:None, 1:CompanionComputer, 2:IRLock
+    // @Values: 0:None, 1:CompanionComputer, 2:IRLock, 3:SITL_Gazebo
     // @User: Advanced
     AP_GROUPINFO("TYPE",    1, AC_PrecLand, _type, 0),
 
@@ -93,10 +94,15 @@ 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 || CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
         case PRECLAND_TYPE_IRLOCK:
             _backend = new AC_PrecLand_IRLock(*this, _backend_state);
             break;
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+        case PRECLAND_TYPE_SITL_GAZEBO:
+            _backend = new AC_PrecLand_SITL_Gazebo(*this, _backend_state);
+            break;
 #endif
     }
 

libraries/AC_PrecLand/AC_PrecLand.h

@@ -12,6 +12,7 @@
 class AC_PrecLand_Backend;
 class AC_PrecLand_Companion;
 class AC_PrecLand_IRLock;
+class AC_PrecLand_SITL_Gazebo;
 
 class AC_PrecLand
 {
@@ -19,6 +20,7 @@ class AC_PrecLand
     friend class AC_PrecLand_Backend;
     friend class AC_PrecLand_Companion;
     friend class AC_PrecLand_IRLock;
+    friend class AC_PrecLand_SITL_Gazebo;
 
 public:
 
@@ -33,7 +35,8 @@ public:
     enum PrecLandType {
         PRECLAND_TYPE_NONE = 0,
         PRECLAND_TYPE_COMPANION,
-        PRECLAND_TYPE_IRLOCK
+        PRECLAND_TYPE_IRLOCK,
+        PRECLAND_TYPE_SITL_GAZEBO,
     };
 
     // constructor

libraries/AC_PrecLand/AC_PrecLand_IRLock.h

@@ -41,11 +41,7 @@ public:
     bool have_los_meas();
 
 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

libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.cpp

@@ -0,0 +1,58 @@
+#include <AP_HAL/AP_HAL.h>
+#include "AC_PrecLand_SITL_Gazebo.h"
+
+extern const AP_HAL::HAL& hal;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+
+// Constructor
+AC_PrecLand_SITL_Gazebo::AC_PrecLand_SITL_Gazebo(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state)
+    : AC_PrecLand_Backend(frontend, state),
+      irlock()
+{
+}
+
+// init - perform initialisation of this backend
+void AC_PrecLand_SITL_Gazebo::init()
+{
+    irlock.init();
+}
+
+// update - give chance to driver to get updates from sensor
+void AC_PrecLand_SITL_Gazebo::update()
+{
+    // update health
+    _state.healthy = irlock.healthy();
+
+    // get new sensor data
+    irlock.update();
+
+    if (irlock.num_targets() > 0 && irlock.last_update_ms() != _los_meas_time_ms) {
+        irlock.get_unit_vector_body(_los_meas_body);
+        _have_los_meas = true;
+        _los_meas_time_ms = irlock.last_update_ms();
+    }
+    _have_los_meas = _have_los_meas && AP_HAL::millis()-_los_meas_time_ms <= 1000;
+}
+
+// provides a unit vector towards the target in body frame
+//  returns same as have_los_meas()
+bool AC_PrecLand_SITL_Gazebo::get_los_body(Vector3f& ret) {
+    if (have_los_meas()) {
+        ret = _los_meas_body;
+        return true;
+    }
+    return false;
+}
+
+// returns system time in milliseconds of last los measurement
+uint32_t AC_PrecLand_SITL_Gazebo::los_meas_time_ms() {
+    return _los_meas_time_ms;
+}
+
+// return true if there is a valid los measurement available
+bool AC_PrecLand_SITL_Gazebo::have_los_meas() {
+    return _have_los_meas;
+}
+
+#endif

libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h

@@ -0,0 +1,45 @@
+#pragma once
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include <AP_Common/AP_Common.h>
+#include <AP_Math/AP_Math.h>
+#include <AC_PrecLand/AC_PrecLand_Backend.h>
+#include <AP_IRLock/AP_IRLock_SITL.h>
+
+/*
+ * AC_PrecLand_SITL_Gazebo - implements precision landing using target
+ * vectors provided Gazebo via a network socket
+ */
+
+class AC_PrecLand_SITL_Gazebo : public AC_PrecLand_Backend
+{
+public:
+
+    // Constructor
+    AC_PrecLand_SITL_Gazebo(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state);
+
+    // perform any required initialisation of backend
+    void init();
+
+    // retrieve updates from sensor
+    void update();
+
+    // provides a unit vector towards the target in body frame
+    //  returns same as have_los_meas()
+    bool get_los_body(Vector3f& ret);
+
+    // returns system time in milliseconds of last los measurement
+    uint32_t los_meas_time_ms();
+
+    // return true if there is a valid los measurement available
+    bool have_los_meas();
+
+private:
+    AP_IRLock_SITL irlock;
+
+    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
+};
+
+#endif