From 998119d011e22ef2c190b12fd66836fcdce12207 Mon Sep 17 00:00:00 2001 From: Shiv Tyagi Date: Tue, 2 Aug 2022 15:38:49 +0530 Subject: [PATCH] SITL: fix var naming for precland device center --- libraries/SITL/SIM_Aircraft.cpp | 2 +- libraries/SITL/SIM_Precland.cpp | 48 ++++++++++++++++----------------- libraries/SITL/SIM_Precland.h | 6 ++--- 3 files changed, 28 insertions(+), 28 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index a4d64ad719..f0528dd30d 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -989,7 +989,7 @@ void Aircraft::update_external_payload(const struct sitl_input &input) if (precland && precland->is_enabled()) { precland->update(get_location(), get_position_relhome()); if (precland->_over_precland_base) { - local_ground_level += precland->_origin_height; + local_ground_level += precland->_device_height; } } diff --git a/libraries/SITL/SIM_Precland.cpp b/libraries/SITL/SIM_Precland.cpp index fe98c90bf9..cfbf91301f 100644 --- a/libraries/SITL/SIM_Precland.cpp +++ b/libraries/SITL/SIM_Precland.cpp @@ -32,31 +32,31 @@ const AP_Param::GroupInfo SIM_Precland::var_info[] = { AP_GROUPINFO("ENABLE", 0, SIM_Precland, _enable, 0), // @Param: LAT - // @DisplayName: Precland device origin's latitude - // @Description: Precland device origin's latitude + // @DisplayName: Precland device center's latitude + // @Description: Precland device center's latitude // @Units: deg // @Increment: 0.000001 // @Range: -90 90 // @User: Advanced - AP_GROUPINFO("LAT", 1, SIM_Precland, _origin_lat, 0), + AP_GROUPINFO("LAT", 1, SIM_Precland, _device_lat, 0), // @Param: LON - // @DisplayName: Precland device origin's longitude - // @Description: Precland device origin's longitude + // @DisplayName: Precland device center's longitude + // @Description: Precland device center's longitude // @Units: deg // @Increment: 0.000001 // @Range: -180 180 // @User: Advanced - AP_GROUPINFO("LON", 2, SIM_Precland, _origin_lon, 0), + AP_GROUPINFO("LON", 2, SIM_Precland, _device_lon, 0), // @Param: HEIGHT - // @DisplayName: Precland device origin's height above sealevel - // @Description: Precland device origin's height above sealevel assume a 2x2m square as station base + // @DisplayName: Precland device center's height above sealevel + // @Description: Precland device center's height above sealevel assume a 2x2m square as station base // @Units: cm // @Increment: 1 // @Range: 0 10000 // @User: Advanced - AP_GROUPINFO("HEIGHT", 3, SIM_Precland, _origin_height, 0), + AP_GROUPINFO("HEIGHT", 3, SIM_Precland, _device_height, 0), // @Param: YAW // @DisplayName: Precland device systems rotation from north @@ -119,36 +119,36 @@ void SIM_Precland::update(const Location &loc, const Vector3d &position) return; } - const Location origin_center(static_cast(_origin_lat * 1.0e7f), - static_cast(_origin_lon * 1.0e7f), - static_cast(_origin_height), + const Location device_center(static_cast(_device_lat * 1.0e7f), + static_cast(_device_lon * 1.0e7f), + static_cast(_device_height), Location::AltFrame::ABOVE_HOME); Vector2f centerf; - if (!origin_center.get_vector_xy_from_origin_NE(centerf)) { + if (!device_center.get_vector_xy_from_origin_NE(centerf)) { _healthy = false; return; } centerf = centerf * 0.01f; // cm to m - Vector3d center(centerf.x, centerf.y, -_origin_height); // convert to make the further vector operations easy + Vector3d center(centerf.x, centerf.y, -_device_height); // convert to make the further vector operations easy // 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_origin = position - center; // position of vehicle with respect to preland device origin + Vector3d position_wrt_device = position - center; // 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 origin 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 // this plane is the ground plane when the axis has PITCH_90 rotation - Vector3d projection_on_axis = position_wrt_origin.projected(axis); + Vector3d projection_on_axis = position_wrt_device.projected(axis); const float longitudinal_dist = projection_on_axis.length(); // lateral distance of vehicle from the precland device // this is the perpendicular distance of vehicle from the axis of cone/cylinder - const float lateral_distance = safe_sqrt(MAX(0, position_wrt_origin.length_squared() - longitudinal_dist*longitudinal_dist)); + const float lateral_distance = safe_sqrt(MAX(0, position_wrt_device.length_squared() - longitudinal_dist*longitudinal_dist)); // sign of projection's dot product with axis tells if vehicle is in front of beacon // return false if vehicle if vehicle is longitudinally too far away from precland device - // for PITCH_90 orientation, longitudinal distance = alt of vehicle - origin_height (in m) + // for PITCH_90 orientation, longitudinal distance = alt of vehicle - device_height (in m) if (projection_on_axis.dot(axis) <= 0 || longitudinal_dist > _alt_limit) { _healthy = false; return; @@ -172,7 +172,7 @@ void SIM_Precland::update(const Location &loc, const Vector3d &position) break; } case PRECLAND_TYPE_SPHERE: { - if (position_wrt_origin.length() > _dist_limit) { + if (position_wrt_device.length() > _dist_limit) { _healthy = false; return; } @@ -187,14 +187,14 @@ void SIM_Precland::update(const Location &loc, const Vector3d &position) break; } } - _target_pos = position_wrt_origin; + _target_pos = position_wrt_device; _healthy = true; } void SIM_Precland::set_default_location(float lat, float lon, int16_t yaw) { - if (is_zero(_origin_lat) && is_zero(_origin_lon)) { - _origin_lat.set(lat); - _origin_lon.set(lon); + if (is_zero(_device_lat) && is_zero(_device_lon)) { + _device_lat.set(lat); + _device_lon.set(lon); _orient_yaw.set(yaw); } } diff --git a/libraries/SITL/SIM_Precland.h b/libraries/SITL/SIM_Precland.h index 90b5803070..827e83a5d5 100644 --- a/libraries/SITL/SIM_Precland.h +++ b/libraries/SITL/SIM_Precland.h @@ -42,9 +42,9 @@ public: static const struct AP_Param::GroupInfo var_info[]; AP_Int8 _enable; - AP_Float _origin_lat; - AP_Float _origin_lon; - AP_Float _origin_height; + AP_Float _device_lat; + AP_Float _device_lon; + AP_Float _device_height; AP_Int16 _orient_yaw; AP_Int8 _type; AP_Int32 _rate;