Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/SITL/SIM_Precland.cpp

230 lines
8.1 KiB
C++
Raw Blame History

/*
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 <http://www.gnu.org/licenses/>.
*/
#include "SIM_config.h"
#include "SIM_Precland.h"
#include "AP_HAL/AP_HAL.h"
#include "AP_Math/AP_Math.h"
#include "AP_Common/Location.h"
#include "SITL.h"
#include <stdio.h>
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),
#if AP_SIM_SHIP_ENABLED
// @Param: SHIP
// @DisplayName: SIM_Precland follow ship
// @Description: This makes the position of the landing beacon follow the simulated ship from SIM_SHIP. The ship movement is controlled with the SIM_SHIP parameters
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO("SHIP", 11, SIM_Precland, _ship, 0),
#endif
AP_GROUPEND
};
void SIM_Precland::update(const Location &loc)
{
if (!_enable) {
_healthy = false;
return;
}
if (is_zero(_alt_limit) || _dist_limit < 1.0f) {
_healthy = false;
return;
}
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*100),
Location::AltFrame::ABOVE_ORIGIN);
#if AP_SIM_SHIP_ENABLED
if (_ship == 1) {
/*
make precland target follow the simulated ship if the ship is enabled
*/
auto *sitl = AP::sitl();
Location shiploc;
if (sitl != nullptr && sitl->shipsim.get_location(shiploc) && !shiploc.is_zero()) {
shiploc.change_alt_frame(Location::AltFrame::ABOVE_ORIGIN);
device_center = shiploc;
}
}
#endif
// 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
device_center.change_alt_frame(loc.get_alt_frame());
Vector3d position_wrt_device = device_center.get_distance_NED_double(loc); // 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);
}
}
Reference in New Issue View Git Blame Copy Permalink