aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: fix var naming for precland device center

Browse Source
This commit is contained in:
Shiv Tyagi 2022-08-02 15:38:49 +05:30 committed by Peter Barker
parent 9a450a05bb
commit 998119d011
3 changed files with 28 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/SITL/SIM_Aircraft.cpp
View File

@ -989,7 +989,7 @@ void Aircraft::update_external_payload(const struct sitl_input &input)
if (precland && precland->is_enabled()) { if (precland && precland->is_enabled()) {
precland->update(get_location(), get_position_relhome()); precland->update(get_location(), get_position_relhome());
if (precland->_over_precland_base) { if (precland->_over_precland_base) {
local_ground_level += precland->_origin_height; local_ground_level += precland->_device_height;
} }
} }

48
libraries/SITL/SIM_Precland.cpp
View File

@ -32,31 +32,31 @@ const AP_Param::GroupInfo SIM_Precland::var_info[] = {
AP_GROUPINFO("ENABLE", 0, SIM_Precland, _enable, 0), AP_GROUPINFO("ENABLE", 0, SIM_Precland, _enable, 0),
// @Param: LAT // @Param: LAT
// @DisplayName: Precland device origin's latitude // @DisplayName: Precland device center's latitude
// @Description: Precland device origin's latitude // @Description: Precland device center's latitude
// @Units: deg // @Units: deg
// @Increment: 0.000001 // @Increment: 0.000001
// @Range: -90 90 // @Range: -90 90
// @User: Advanced // @User: Advanced
AP_GROUPINFO("LAT", 1, SIM_Precland, _origin_lat, 0), AP_GROUPINFO("LAT", 1, SIM_Precland, _device_lat, 0),
// @Param: LON // @Param: LON
// @DisplayName: Precland device origin's longitude // @DisplayName: Precland device center's longitude
// @Description: Precland device origin's longitude // @Description: Precland device center's longitude
// @Units: deg // @Units: deg
// @Increment: 0.000001 // @Increment: 0.000001
// @Range: -180 180 // @Range: -180 180
// @User: Advanced // @User: Advanced
AP_GROUPINFO("LON", 2, SIM_Precland, _origin_lon, 0), AP_GROUPINFO("LON", 2, SIM_Precland, _device_lon, 0),
// @Param: HEIGHT // @Param: HEIGHT
// @DisplayName: Precland device origin's height above sealevel // @DisplayName: Precland device center's height above sealevel
// @Description: Precland device origin's height above sealevel assume a 2x2m square as station base // @Description: Precland device center's height above sealevel assume a 2x2m square as station base
// @Units: cm // @Units: cm
// @Increment: 1 // @Increment: 1
// @Range: 0 10000 // @Range: 0 10000
// @User: Advanced // @User: Advanced
AP_GROUPINFO("HEIGHT", 3, SIM_Precland, _origin_height, 0), AP_GROUPINFO("HEIGHT", 3, SIM_Precland, _device_height, 0),
// @Param: YAW // @Param: YAW
// @DisplayName: Precland device systems rotation from north // @DisplayName: Precland device systems rotation from north
@ -119,36 +119,36 @@ void SIM_Precland::update(const Location &loc, const Vector3d &position)
return; return;
} }
const Location origin_center(static_cast<int32_t>(_origin_lat * 1.0e7f), const Location device_center(static_cast<int32_t>(_device_lat * 1.0e7f),
static_cast<int32_t>(_origin_lon * 1.0e7f), static_cast<int32_t>(_device_lon * 1.0e7f),
static_cast<int32_t>(_origin_height), static_cast<int32_t>(_device_height),
Location::AltFrame::ABOVE_HOME); Location::AltFrame::ABOVE_HOME);
Vector2f centerf; Vector2f centerf;
if (!origin_center.get_vector_xy_from_origin_NE(centerf)) { if (!device_center.get_vector_xy_from_origin_NE(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, -_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 // 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_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 // 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 // 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(); const float longitudinal_dist = projection_on_axis.length();
// lateral distance of vehicle from the precland device // lateral distance of vehicle from the precland device
// this is the perpendicular distance of vehicle from the axis of cone/cylinder // 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 // 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 // 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) { if (projection_on_axis.dot(axis) <= 0 || longitudinal_dist > _alt_limit) {
_healthy = false; _healthy = false;
return; return;
@ -172,7 +172,7 @@ void SIM_Precland::update(const Location &loc, const Vector3d &position)
break; break;
} }
case PRECLAND_TYPE_SPHERE: { case PRECLAND_TYPE_SPHERE: {
if (position_wrt_origin.length() > _dist_limit) { if (position_wrt_device.length() > _dist_limit) {
_healthy = false; _healthy = false;
return; return;
} }
@ -187,14 +187,14 @@ void SIM_Precland::update(const Location &loc, const Vector3d &position)
break; break;
} }
} }
_target_pos = position_wrt_origin; _target_pos = position_wrt_device;
_healthy = true; _healthy = true;
} }
void SIM_Precland::set_default_location(float lat, float lon, int16_t yaw) { void SIM_Precland::set_default_location(float lat, float lon, int16_t yaw) {
if (is_zero(_origin_lat) && is_zero(_origin_lon)) { if (is_zero(_device_lat) && is_zero(_device_lon)) {
_origin_lat.set(lat); _device_lat.set(lat);
_origin_lon.set(lon); _device_lon.set(lon);
_orient_yaw.set(yaw); _orient_yaw.set(yaw);
} }
} }

6
libraries/SITL/SIM_Precland.h
View File

@ -42,9 +42,9 @@ public:
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
AP_Int8 _enable; AP_Int8 _enable;
AP_Float _origin_lat; AP_Float _device_lat;
AP_Float _origin_lon; AP_Float _device_lon;
AP_Float _origin_height; AP_Float _device_height;
AP_Int16 _orient_yaw; AP_Int16 _orient_yaw;
AP_Int8 _type; AP_Int8 _type;
AP_Int32 _rate; AP_Int32 _rate;