AC_Precland: use SITL precland object

AC_Precland: simplify includes

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -1,5 +1,6 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Scheduler/AP_Scheduler.h>
+#include <AP_AHRS/AP_AHRS.h>
 #include "AC_PrecLand.h"
 #include "AC_PrecLand_Backend.h"
 #include "AC_PrecLand_Companion.h"

libraries/AC_PrecLand/AC_PrecLand.h

@@ -1,6 +1,5 @@
 #pragma once
 
-#include <AP_Common/AP_Common.h>
 #include <AP_Math/AP_Math.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <stdint.h>

libraries/AC_PrecLand/AC_PrecLand_Backend.h

@@ -1,6 +1,5 @@
 #pragma once
 
-#include <AP_Common/AP_Common.h>
 #include <AP_Math/AP_Math.h>
 #include <AC_PID/AC_PID.h>
 #include "AC_PrecLand.h"

libraries/AC_PrecLand/AC_PrecLand_Companion.cpp

@@ -1,8 +1,6 @@
 #include <AP_HAL/AP_HAL.h>
 #include "AC_PrecLand_Companion.h"
 
-extern const AP_HAL::HAL& hal;
-
 // perform any required initialisation of backend
 void AC_PrecLand_Companion::init()
 {

libraries/AC_PrecLand/AC_PrecLand_Companion.h

@@ -1,6 +1,5 @@
 #pragma once
 
-#include <AP_Common/AP_Common.h>
 #include <AP_Math/AP_Math.h>
 #include "AC_PrecLand_Backend.h"
 

libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp

@@ -1,8 +1,6 @@
 #include <AP_HAL/AP_HAL.h>
 #include "AC_PrecLand_IRLock.h"
 
-extern const AP_HAL::HAL& hal;
-
 // Constructor
 AC_PrecLand_IRLock::AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state)
     : AC_PrecLand_Backend(frontend, state),

libraries/AC_PrecLand/AC_PrecLand_IRLock.h

@@ -1,12 +1,11 @@
 #pragma once
 
-#include <AP_Common/AP_Common.h>
 #include <AP_Math/AP_Math.h>
 #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>
+ #include <AP_IRLock/AP_IRLock_SITL.h>
+#else
+ #include <AP_IRLock/AP_IRLock.h>
 #endif
 
 /*
@@ -38,8 +37,11 @@ public:
     bool have_los_meas() override;
 
 private:
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    AP_IRLock_SITL irlock;
+#else
     AP_IRLock_I2C 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/AC_PrecLand/AC_PrecLand_SITL.cpp

@@ -1,41 +1,32 @@
 #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>
-
+#include "AP_AHRS/AP_AHRS.h"
 // init - perform initialisation of this backend
 void AC_PrecLand_SITL::init()
 {
+    _sitl = AP::sitl();
 }
 
 // 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 (! AP::ahrs().get_relative_position_NED_home(home)) {
-        _state.healthy = false;
-        return;
-    }
-    if (home.length() > 10.0f) { // we can see the target out to 10 metres
-        return;
+    _state.healthy = _sitl->precland_sim.healthy();
+
+    if (_state.healthy && _sitl->precland_sim.last_update_ms() != _los_meas_time_ms) {
+        const Vector3f position = _sitl->precland_sim.get_target_position();
+        const Matrix3f &body_to_ned = AP::ahrs().get_rotation_body_to_ned();
+        _los_meas_body =  body_to_ned.mul_transpose(-position);
+        _los_meas_body /= _los_meas_body.length();
+        _have_los_meas = true;
+        _los_meas_time_ms = _sitl->precland_sim.last_update_ms();
+    } else {
+        _have_los_meas = false;
     }
-    _state.healthy = true;
-
-    const Matrix3f &body_to_ned = AP::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;
+    _have_los_meas = _have_los_meas && AP_HAL::millis() - _los_meas_time_ms <= 1000;
 }
 
 bool AC_PrecLand_SITL::have_los_meas() {

libraries/AC_PrecLand/AC_PrecLand_SITL.h

@@ -1,9 +1,9 @@
 #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 <SITL/SITL.h>
 
 /*
  * AC_PrecLand_SITL - supplies vectors to a fake landing target
@@ -33,9 +33,10 @@ public:
     bool have_los_meas() override;
 
 private:
-
+    SITL::SITL          *_sitl;                 // sitl instance pointer
     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
+    bool                _have_los_meas;         // true if there is a valid measurement from the camera
 };
 
 #endif

libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h

@@ -1,10 +1,9 @@
 #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>
+#include <AP_IRLock/AP_IRLock_SITL_Gazebo.h>
 
 /*
  * AC_PrecLand_SITL_Gazebo - implements precision landing using target
@@ -35,7 +34,7 @@ public:
     bool have_los_meas() override;
 
 private:
-    AP_IRLock_SITL irlock;
+    AP_IRLock_SITL_Gazebo 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