diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp b/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp index 0ed7ca7e8f..99a3261a29 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp @@ -19,6 +19,7 @@ void AC_PrecLand_SITL::update() const Vector3d position = _sitl->precland_sim.get_target_position(); const Matrix3d body_to_ned = AP::ahrs().get_rotation_body_to_ned().todouble(); _los_meas_body = body_to_ned.mul_transpose(-position).tofloat(); + _distance_to_target = _sitl->precland_sim.option_enabled(SITL::SIM_Precland::Option::ENABLE_TARGET_DISTANCE) ? _los_meas_body.length() : 0.0f; _los_meas_body /= _los_meas_body.length(); _have_los_meas = true; _los_meas_time_ms = _sitl->precland_sim.last_update_ms(); @@ -33,7 +34,6 @@ 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) { diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL.h b/libraries/AC_PrecLand/AC_PrecLand_SITL.h index 82e2efc97b..ee6fd526ed 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_SITL.h +++ b/libraries/AC_PrecLand/AC_PrecLand_SITL.h @@ -32,11 +32,15 @@ public: // return true if there is a valid los measurement available bool have_los_meas() override; + // returns distance to target in meters (0 means distance is not known) + float distance_to_target() override { return _distance_to_target; } + private: SITL::SIM *_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 + float _distance_to_target; // distance to target in meters }; #endif