Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-24 00:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: fixed precland simulator

Browse Source 
fixed units of height, was mixed cm and m, now just m AMSL
This commit is contained in:
Andrew Tridgell 2024-02-25 17:30:36 +11:00 committed by Peter Barker
parent 57573f7d63
commit 205fb22674
1 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/SITL/SIM_Precland.cpp
View File

@ -50,9 +50,9 @@ const AP_Param::GroupInfo SIM_Precland::var_info[] = {
AP_GROUPINFO("LON", 2, SIM_Precland, _device_lon, 0), AP_GROUPINFO("LON", 2, SIM_Precland, _device_lon, 0),
// @Param: HEIGHT // @Param: HEIGHT
// @DisplayName: Precland device center's height above sealevel // @DisplayName: Precland device center's height SITL origin
// @Description: Precland device center's height above sealevel assume a 2x2m square as station base // @Description: Precland device center's height above SITL origin. Assumes a 2x2m square as station base
// @Units: cm // @Units: m
// @Increment: 1 // @Increment: 1
// @Range: 0 10000 // @Range: 0 10000
// @User: Advanced // @User: Advanced
@ -115,7 +115,7 @@ const AP_Param::GroupInfo SIM_Precland::var_info[] = {
AP_GROUPEND AP_GROUPEND
}; };
void SIM_Precland::update(const Location &loc, const Vector3d &position) void SIM_Precland::update(const Location &loc, const Vector3d &position_relhome)
{ {
if (!_enable) { if (!_enable) {
_healthy = false; _healthy = false;
@ -127,21 +127,21 @@ void SIM_Precland::update(const Location &loc, const Vector3d &position)
} }
const Location device_center(static_cast<int32_t>(_device_lat * 1.0e7f), const Location device_center(static_cast<int32_t>(_device_lat * 1.0e7f),
static_cast<int32_t>(_device_lon * 1.0e7f), static_cast<int32_t>(_device_lon * 1.0e7f),
static_cast<int32_t>(_device_height), static_cast<int32_t>(_device_height*100),
Location::AltFrame::ABOVE_HOME); Location::AltFrame::ABOVE_ORIGIN);
Vector2f centerf; Vector3f centerf;
if (!device_center.get_vector_xy_from_origin_NE(centerf)) { if (!device_center.get_vector_from_origin_NEU(centerf)) {
_healthy = false; _healthy = false;
return; return;
} }
centerf = centerf * 0.01f; // cm to m centerf = centerf * 0.01f; // cm to m
Vector3d center(centerf.x, centerf.y, -_device_height); // convert to make the further vector operations easy centerf.z *= -1; // NEU to NED
// axis of cone or cylinder inside which the vehicle receives signals from simulated precland device // axis of cone or cylinder inside which the vehicle receives signals from simulated precland device
Vector3d axis{1, 0, 0}; Vector3d axis{1, 0, 0};
axis.rotate((Rotation)_orient.get()); // unit vector in direction of axis of cone or cylinder axis.rotate((Rotation)_orient.get()); // unit vector in direction of axis of cone or cylinder
Vector3d position_wrt_device = position - center; // position of vehicle with respect to preland device center Vector3d position_wrt_device = position_relhome - centerf.todouble(); // position of vehicle with respect to preland device center
// longitudinal distance of vehicle from the precland device // 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 // this is the distance of vehicle from the plane which is passing through precland device center and perpendicular to axis of cone/cylinder