diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index b0f1fa6d20..2790c692e7 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -1001,7 +1001,7 @@ void Aircraft::update_external_payload(const struct sitl_input &input)
}
if (precland && precland->is_enabled()) {
- precland->update(get_location(), get_position_relhome());
+ precland->update(get_location());
if (precland->_over_precland_base) {
local_ground_level += precland->_device_height;
}
diff --git a/libraries/SITL/SIM_Precland.cpp b/libraries/SITL/SIM_Precland.cpp
index 7bb40f8ea3..50af3960cd 100644
--- a/libraries/SITL/SIM_Precland.cpp
+++ b/libraries/SITL/SIM_Precland.cpp
@@ -13,10 +13,13 @@
along with this program. If not, see .
*/
+#include "SIM_config.h"
+
#include "SIM_Precland.h"
#include "AP_HAL/AP_HAL.h"
#include "AP_Math/AP_Math.h"
#include "AP_Common/Location.h"
+#include "SITL.h"
#include
using namespace SITL;
@@ -112,10 +115,19 @@ const AP_Param::GroupInfo SIM_Precland::var_info[] = {
// @User: Advanced
AP_GROUPINFO("OPTIONS", 10, SIM_Precland, _options, 0),
+#if AP_SIM_SHIP_ENABLED
+ // @Param: SHIP
+ // @DisplayName: SIM_Precland follow ship
+ // @Description: This makes the position of the landing beacon follow the simulated ship from SIM_SHIP. The ship movement is controlled with the SIM_SHIP parameters
+ // @Values: 0:Disabled,1:Enabled
+ // @User: Advanced
+ AP_GROUPINFO("SHIP", 11, SIM_Precland, _ship, 0),
+#endif
+
AP_GROUPEND
};
-void SIM_Precland::update(const Location &loc, const Vector3d &position_relhome)
+void SIM_Precland::update(const Location &loc)
{
if (!_enable) {
_healthy = false;
@@ -126,22 +138,32 @@ void SIM_Precland::update(const Location &loc, const Vector3d &position_relhome)
return;
}
- const Location device_center(static_cast(_device_lat * 1.0e7f),
- static_cast(_device_lon * 1.0e7f),
- static_cast(_device_height*100),
- Location::AltFrame::ABOVE_ORIGIN);
- Vector3f centerf;
- if (!device_center.get_vector_from_origin_NEU(centerf)) {
- _healthy = false;
- return;
+ Location device_center(static_cast(_device_lat * 1.0e7f),
+ static_cast(_device_lon * 1.0e7f),
+ static_cast(_device_height*100),
+ Location::AltFrame::ABOVE_ORIGIN);
+
+#if AP_SIM_SHIP_ENABLED
+ if (_ship == 1) {
+ /*
+ make precland target follow the simulated ship if the ship is enabled
+ */
+ auto *sitl = AP::sitl();
+ Location shiploc;
+ if (sitl != nullptr && sitl->shipsim.get_location(shiploc) && !shiploc.is_zero()) {
+ shiploc.change_alt_frame(Location::AltFrame::ABOVE_ORIGIN);
+ device_center = shiploc;
+ }
}
- centerf = centerf * 0.01f; // cm to m
- centerf.z *= -1; // NEU to NED
+#endif
// axis of cone or cylinder inside which the vehicle receives signals from simulated precland device
Vector3d axis{1, 0, 0};
axis.rotate((Rotation)_orient.get()); // unit vector in direction of axis of cone or cylinder
- Vector3d position_wrt_device = position_relhome - centerf.todouble(); // position of vehicle with respect to preland device center
+
+ device_center.change_alt_frame(loc.get_alt_frame());
+
+ Vector3d position_wrt_device = device_center.get_distance_NED_double(loc); // position of vehicle with respect to preland device center
// longitudinal distance of vehicle from the precland device
// this is the distance of vehicle from the plane which is passing through precland device center and perpendicular to axis of cone/cylinder
diff --git a/libraries/SITL/SIM_Precland.h b/libraries/SITL/SIM_Precland.h
index f779867e6f..9574801daa 100644
--- a/libraries/SITL/SIM_Precland.h
+++ b/libraries/SITL/SIM_Precland.h
@@ -28,7 +28,7 @@ public:
};
// update precland state
- void update(const Location &loc, const Vector3d &position);
+ void update(const Location &loc);
// true if precland sensor is online and healthy
bool healthy() const { return _healthy; }
@@ -56,7 +56,9 @@ public:
AP_Float _alt_limit;
AP_Float _dist_limit;
AP_Int8 _orient;
+ AP_Int8 _ship;
AP_Enum