/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ #include "SIM_Precland.h" #include "AP_HAL/AP_HAL.h" #include "AP_Math/AP_Math.h" #include "AP_Common/Location.h" #include using namespace SITL; // table of user settable parameters const AP_Param::GroupInfo SIM_Precland::var_info[] = { // @Param: ENABLE // @DisplayName: Preland device Sim enable/disable // @Description: Allows you to enable (1) or disable (0) the Preland simulation // @Values: 0:Disabled,1:Enabled // @User: Advanced AP_GROUPINFO("ENABLE", 0, SIM_Precland, _enable, 0), // @Param: LAT // @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, _device_lat, 0), // @Param: LON // @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, _device_lon, 0), // @Param: HEIGHT // @DisplayName: Precland device center's height SITL origin // @Description: Precland device center's height above SITL origin. Assumes a 2x2m square as station base // @Units: m // @Increment: 1 // @Range: 0 10000 // @User: Advanced AP_GROUPINFO("HEIGHT", 3, SIM_Precland, _device_height, 0), // @Param: YAW // @DisplayName: Precland device systems rotation from north // @Description: Precland device systems rotation from north // @Units: deg // @Increment: 1 // @Range: -180 +180 // @User: Advanced AP_GROUPINFO("YAW", 4, SIM_Precland, _orient_yaw, 0), // @Param: RATE // @DisplayName: Precland device update rate // @Description: Precland device rate. e.g led patter refresh rate, RF message rate, etc. // @Units: Hz // @Range: 0 200 // @User: Advanced AP_GROUPINFO("RATE", 5, SIM_Precland, _rate, 100), // @Param: TYPE // @DisplayName: Precland device radiance type // @Description: Precland device radiance type: it can be a cylinder, a cone, or a sphere. // @Values: 0:cylinder,1:cone,2:sphere // @User: Advanced AP_GROUPINFO("TYPE", 6, SIM_Precland, _type, SIM_Precland::PRECLAND_TYPE_CYLINDER), // @Param: ALT_LIMIT // @DisplayName: Precland device alt range // @Description: Precland device maximum range altitude // @Units: m // @Range: 0 100 // @User: Advanced AP_GROUPINFO("ALT_LMT", 7, SIM_Precland, _alt_limit, 15), // @Param: DIST_LIMIT // @DisplayName: Precland device lateral range // @Description: Precland device maximum lateral range // @Units: m // @Range: 5 100 // @User: Advanced AP_GROUPINFO("DIST_LMT", 8, SIM_Precland, _dist_limit, 10), // @Param: ORIENT // @DisplayName: Precland device orientation // @Description: Precland device orientation vector // @Values: 0:Front, 4:Back, 24:Up // @User: Advanced AP_GROUPINFO("ORIENT", 9, SIM_Precland, _orient, ROTATION_PITCH_90), // @Param: OPTIONS // @DisplayName: SIM_Precland extra options // @Description: SIM_Precland extra options // @Bitmask: 0: Enable target distance // @User: Advanced AP_GROUPINFO("OPTIONS", 10, SIM_Precland, _options, 0), AP_GROUPEND }; void SIM_Precland::update(const Location &loc, const Vector3d &position_relhome) { if (!_enable) { _healthy = false; return; } if (is_zero(_alt_limit) || _dist_limit < 1.0f) { _healthy = false; 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; } centerf = centerf * 0.01f; // cm to m centerf.z *= -1; // NEU to NED // 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 // 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 plane is the ground plane when the axis has PITCH_90 rotation 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_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 - device_height (in m) if (projection_on_axis.dot(axis) <= 0 || longitudinal_dist > _alt_limit) { _healthy = false; return; } const uint32_t now = AP_HAL::millis(); if (now - _last_update_ms < 1000.0f * (1.0f / _rate)) { return; } _last_update_ms = now; switch (_type) { case PRECLAND_TYPE_CONE: { // lateral_limit is the limit of how far the vehicle can laterally be from precland_device // in case of cone, this limit increases gradually as the vehicle moves longitudinally far away from precland device const float lateral_limit = longitudinal_dist * _dist_limit / _alt_limit; if (lateral_distance > lateral_limit) { _healthy = false; return; } break; } case PRECLAND_TYPE_SPHERE: { if (position_wrt_device.length() > _dist_limit) { _healthy = false; return; } break; } default: case PRECLAND_TYPE_CYLINDER: { if (lateral_distance > _dist_limit) { _healthy = false; return; } break; } } _target_pos = position_wrt_device; _healthy = true; } void SIM_Precland::set_default_location(float lat, float lon, int16_t yaw) { if (is_zero(_device_lat) && is_zero(_device_lon)) { _device_lat.set(lat); _device_lon.set(lon); _orient_yaw.set(yaw); } }