|
|
|
@ -32,31 +32,31 @@ const AP_Param::GroupInfo SIM_Precland::var_info[] = {
@@ -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)
@@ -119,36 +119,36 @@ void SIM_Precland::update(const Location &loc, const Vector3d &position)
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
const Location origin_center(static_cast<int32_t>(_origin_lat * 1.0e7f), |
|
|
|
|
static_cast<int32_t>(_origin_lon * 1.0e7f), |
|
|
|
|
static_cast<int32_t>(_origin_height), |
|
|
|
|
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_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)
@@ -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)
@@ -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); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|