diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 15bed24d0c..e0e8bc93af 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -1,5 +1,6 @@ #include #include +#include #include "AC_PrecLand.h" #include "AC_PrecLand_Backend.h" #include "AC_PrecLand_Companion.h" diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index bf9b8205ec..9f60a8e54b 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -1,6 +1,5 @@ #pragma once -#include #include #include #include diff --git a/libraries/AC_PrecLand/AC_PrecLand_Backend.h b/libraries/AC_PrecLand/AC_PrecLand_Backend.h index f79443dc74..84ea4016a4 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Backend.h +++ b/libraries/AC_PrecLand/AC_PrecLand_Backend.h @@ -1,6 +1,5 @@ #pragma once -#include #include #include #include "AC_PrecLand.h" diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp index a0fb965230..ab82813146 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp @@ -1,8 +1,6 @@ #include #include "AC_PrecLand_Companion.h" -extern const AP_HAL::HAL& hal; - // perform any required initialisation of backend void AC_PrecLand_Companion::init() { diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.h b/libraries/AC_PrecLand/AC_PrecLand_Companion.h index 85c350d91b..11b5b7a0a1 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.h +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.h @@ -1,6 +1,5 @@ #pragma once -#include #include #include "AC_PrecLand_Backend.h" diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp b/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp index 15dd93f50e..04b1126621 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp @@ -1,8 +1,6 @@ #include #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), diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h index bcf78f57c2..da2923b126 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h +++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h @@ -1,12 +1,11 @@ #pragma once -#include #include #include -#include - #if CONFIG_HAL_BOARD == HAL_BOARD_SITL -#include + #include +#else + #include #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 diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp b/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp index 82f7103333..52b806ea5e 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp @@ -1,41 +1,32 @@ #include #include "AC_PrecLand_SITL.h" -extern const AP_HAL::HAL& hal; - #if CONFIG_HAL_BOARD == HAL_BOARD_SITL -#include - +#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() { diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL.h b/libraries/AC_PrecLand/AC_PrecLand_SITL.h index 2b1f7abab7..6821476a54 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_SITL.h +++ b/libraries/AC_PrecLand/AC_PrecLand_SITL.h @@ -1,9 +1,9 @@ #pragma once #if CONFIG_HAL_BOARD == HAL_BOARD_SITL -#include #include #include +#include /* * 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 diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h b/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h index 2e556d9a22..e7f1ab06ed 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h +++ b/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h @@ -1,10 +1,9 @@ #pragma once #if CONFIG_HAL_BOARD == HAL_BOARD_SITL -#include #include #include -#include +#include /* * 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