AC_PrecLand: a pure-ArduPilot simulated SITL precision land backend

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -4,6 +4,7 @@
 #include "AC_PrecLand_Companion.h"
 #include "AC_PrecLand_IRLock.h"
 #include "AC_PrecLand_SITL_Gazebo.h"
+#include "AC_PrecLand_SITL.h"
 
 extern const AP_HAL::HAL& hal;
 
@@ -18,7 +19,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, 3:SITL_Gazebo
+    // @Values: 0:None, 1:CompanionComputer, 2:IRLock, 3:SITL_Gazebo, 4:SITL
     // @User: Advanced
     AP_GROUPINFO("TYPE",    1, AC_PrecLand, _type, 0),
 
@@ -103,6 +104,9 @@ void AC_PrecLand::init()
         case PRECLAND_TYPE_SITL_GAZEBO:
             _backend = new AC_PrecLand_SITL_Gazebo(*this, _backend_state);
             break;
+        case PRECLAND_TYPE_SITL:
+            _backend = new AC_PrecLand_SITL(*this, _backend_state);
+            break;
 #endif
     }
 

libraries/AC_PrecLand/AC_PrecLand.h

@@ -13,6 +13,7 @@ class AC_PrecLand_Backend;
 class AC_PrecLand_Companion;
 class AC_PrecLand_IRLock;
 class AC_PrecLand_SITL_Gazebo;
+class AC_PrecLand_SITL;
 
 class AC_PrecLand
 {
@@ -21,6 +22,7 @@ class AC_PrecLand
     friend class AC_PrecLand_Companion;
     friend class AC_PrecLand_IRLock;
     friend class AC_PrecLand_SITL_Gazebo;
+    friend class AC_PrecLand_SITL;
 
 public:
 
@@ -37,6 +39,7 @@ public:
         PRECLAND_TYPE_COMPANION,
         PRECLAND_TYPE_IRLOCK,
         PRECLAND_TYPE_SITL_GAZEBO,
+        PRECLAND_TYPE_SITL,
     };
 
     // constructor

libraries/AC_PrecLand/AC_PrecLand_SITL.cpp

@@ -0,0 +1,63 @@
+#include <AP_HAL/AP_HAL.h>
+#include "AC_PrecLand_SITL.h"
+
+extern const AP_HAL::HAL& hal;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+
+#include <stdio.h>
+
+// Constructor
+AC_PrecLand_SITL::AC_PrecLand_SITL(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state)
+    : AC_PrecLand_Backend(frontend, state)
+{
+}
+
+// init - perform initialisation of this backend
+void AC_PrecLand_SITL::init()
+{
+}
+
+// update - give chance to driver to get updates from sensor
+void AC_PrecLand_SITL::update()
+{
+    const uint32_t now = AP_HAL::millis();
+    if (_los_meas_time_ms + 10 > now) { // 100Hz update
+        return;
+    }
+
+    // get new sensor data; we always point home
+    Vector3f home;
+    if (! _frontend._ahrs.get_relative_position_NED(home)) {
+        _state.healthy = false;
+        return;
+    }
+    if (home.length() > 10.0f) { // we can see the target out to 10 metres
+        return;
+    }
+    _state.healthy = true;
+
+    const Matrix3f &body_to_ned = _frontend._ahrs.get_rotation_body_to_ned();
+
+    _los_meas_body =  body_to_ned.mul_transpose(-home);
+    _los_meas_body /= _los_meas_body.length();
+    _los_meas_time_ms = now;
+}
+
+bool AC_PrecLand_SITL::have_los_meas() {
+    return 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::get_los_body(Vector3f& ret) {
+    if (AP_HAL::millis() - _los_meas_time_ms > 1000) {
+        // no measurement for a full second; no vector available
+        return false;
+    }
+    ret = _los_meas_body;
+    return true;
+}
+
+#endif

libraries/AC_PrecLand/AC_PrecLand_SITL.h

@@ -0,0 +1,41 @@
+#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>
+
+/*
+ * AC_PrecLand_SITL - supplies vectors to a fake landing target
+ */
+
+class AC_PrecLand_SITL : public AC_PrecLand_Backend
+{
+public:
+
+    // Constructor
+    AC_PrecLand_SITL(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 _los_meas_time_ms; }
+
+    // return true if there is a valid los measurement available
+    bool have_los_meas();
+
+private:
+
+    Vector3f            _los_meas_body;         // unit vector in body frame pointing towards target
+    uint32_t            _los_meas_time_ms;      // system time in milliseconds when los was measured
+};
+
+#endif