From 04bc267a61710796b282fa4868228211e14f9d66 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Fri, 3 Aug 2018 12:52:20 +0200 Subject: [PATCH] SITL: add precland object add position for precland object add refresh rate add alt and lateral range add diffusion range --- libraries/SITL/SIM_Aircraft.cpp | 10 +- libraries/SITL/SIM_Aircraft.h | 6 ++ libraries/SITL/SIM_Precland.cpp | 172 ++++++++++++++++++++++++++++++++ libraries/SITL/SIM_Precland.h | 64 ++++++++++++ libraries/SITL/SITL.cpp | 4 + libraries/SITL/SITL.h | 2 + 6 files changed, 257 insertions(+), 1 deletion(-) create mode 100644 libraries/SITL/SIM_Precland.cpp create mode 100644 libraries/SITL/SIM_Precland.h diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 9b4247501b..b6a97d6729 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -159,6 +159,11 @@ float Aircraft::ground_height_difference() const return 0.0f; } +void Aircraft::set_precland(SIM_Precland *_precland) { + precland = _precland; + precland->set_default_location(home.lat * 1.0e-7f, home.lng * 1.0e-7f, static_cast(get_home_yaw())); +} + /* return current height above ground level (metres) */ @@ -766,10 +771,13 @@ void Aircraft::update_external_payload(const struct sitl_input &input) external_payload_mass += gripper_epm->payload_mass(); } - // update parachute if (parachute && parachute->is_enabled()) { parachute->update(input); // TODO: add drag to vehicle, presumably proportional to velocity } + + if (precland && precland->is_enabled()) { + precland->update(get_location(), get_position()); + } } diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index df9ec7e8f9..c77687dd92 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -27,6 +27,7 @@ #include "SIM_Gripper_Servo.h" #include "SIM_Gripper_EPM.h" #include "SIM_Parachute.h" +#include "SIM_Precland.h" namespace SITL { @@ -109,10 +110,14 @@ public: attitude.from_rotation_matrix(dcm); } + const Location &get_home() const { return home; } + float get_home_yaw() const { return home_yaw; } + void set_sprayer(Sprayer *_sprayer) { sprayer = _sprayer; } void set_parachute(Parachute *_parachute) { parachute = _parachute; } void set_gripper_servo(Gripper_Servo *_gripper) { gripper = _gripper; } void set_gripper_epm(Gripper_EPM *_gripper_epm) { gripper_epm = _gripper_epm; } + void set_precland(SIM_Precland *_precland); protected: SITL *sitl; @@ -261,6 +266,7 @@ private: Gripper_Servo *gripper; Gripper_EPM *gripper_epm; Parachute *parachute; + SIM_Precland *precland; }; } // namespace SITL diff --git a/libraries/SITL/SIM_Precland.cpp b/libraries/SITL/SIM_Precland.cpp new file mode 100644 index 0000000000..32fa54d7f1 --- /dev/null +++ b/libraries/SITL/SIM_Precland.cpp @@ -0,0 +1,172 @@ +/* + 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 origin's latitude + // @Description: Precland device origin's latitude + // @Units: deg + // @Increment: 0.000001 + // @Range: -90 90 + // @User: Advanced + AP_GROUPINFO("LAT", 1, SIM_Precland, _origin_lat, 0), + + // @Param: LON + // @DisplayName: Precland device origin's longitude + // @Description: Precland device origin's longitude + // @Units: deg + // @Increment: 0.000001 + // @Range: -180 180 + // @User: Advanced + AP_GROUPINFO("LON", 2, SIM_Precland, _origin_lon, 0), + + // @Param: HEIGHT + // @DisplayName: Precland device origin's height above sealevel + // @Description: Precland device origin's height above sealevel + // @Units: cm + // @Increment: 1 + // @Range: 0 10000 + // @User: Advanced + AP_GROUPINFO("HEIGHT", 3, SIM_Precland, _origin_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), + + AP_GROUPEND +}; + +void SIM_Precland::update(const Location &loc, const Vector3f &position) +{ + const uint32_t now = AP_HAL::millis(); + + if (!_enable) { + _healthy = false; + return; + } + if (is_zero(_alt_limit) || _dist_limit < 1.0f) { + _healthy = false; + return; + } + + if (now - _last_update_ms < 1000.0f * (1.0f / _rate)) { + return; + } + _last_update_ms = now; + + const float distance_z = -position.z; + const float origin_height_m = _origin_height * 0.01f; + if (distance_z > _alt_limit + origin_height_m) { + _healthy = false; + return; + } + + const Location origin_center(static_cast(_origin_lat * 1.0e7f), + static_cast(_origin_lon * 1.0e7f), + static_cast(_origin_height), + Location::ALT_FRAME_ABOVE_HOME); + Vector2f center; + origin_center.get_vector_xy_from_origin_NE(center); + center = center * 0.01f; // cm to m + + switch (_type) { + case PRECLAND_TYPE_CONE: { + const float in_radius = distance_z * _dist_limit / (_alt_limit + origin_height_m); + if (norm(position.x - center.x, position.y - center.y) > in_radius) { + _healthy = false; + return; + } + break; + } + case PRECLAND_TYPE_SPHERE: { + if (norm(position.x - center.x, position.y - center.y, distance_z - _origin_height) > (_alt_limit + origin_height_m)) { + _healthy = false; + return; + } + break; + } + default: + case PRECLAND_TYPE_CYLINDER: { + if (norm(position.x - center.x, position.y - center.y) > _dist_limit) { + _healthy = false; + return; + } + break; + } + } + _target_pos = position - Vector3f(center.x, center.y, origin_height_m); + _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 = lat; + _origin_lon = lon; + _orient_yaw = yaw; + } +} diff --git a/libraries/SITL/SIM_Precland.h b/libraries/SITL/SIM_Precland.h new file mode 100644 index 0000000000..d9c39d0d7a --- /dev/null +++ b/libraries/SITL/SIM_Precland.h @@ -0,0 +1,64 @@ +/* + 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 . + */ + +#pragma once +#include "stdint.h" +#include +#include + +namespace SITL { + +class SIM_Precland { +public: + SIM_Precland() { + AP_Param::setup_object_defaults(this, var_info); + }; + + // update precland state + void update(const Location &loc, const Vector3f &position); + + // true if precland sensor is online and healthy + bool healthy() const { return _healthy; } + + // timestamp of most recent data read from the sensor + uint32_t last_update_ms() const { return _last_update_ms; } + + const Vector3f &get_target_position() const { return _target_pos; } + bool is_enabled() const {return static_cast(_enable);} + void set_default_location(float lat, float lon, int16_t yaw); + static const struct AP_Param::GroupInfo var_info[]; + + AP_Int8 _enable; + AP_Float _origin_lat; + AP_Float _origin_lon; + AP_Float _origin_height; + AP_Int16 _orient_yaw; + AP_Int8 _type; + AP_Int32 _rate; + AP_Float _alt_limit; + AP_Float _dist_limit; + + enum PreclandType { + PRECLAND_TYPE_CYLINDER = 0, + PRECLAND_TYPE_CONE = 1, + PRECLAND_TYPE_SPHERE = 2, + }; +private: + uint32_t _last_update_ms; + bool _healthy; + Vector3f _target_pos; +}; + +} diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index d1e574c0e8..097b155c85 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -144,6 +144,10 @@ const AP_Param::GroupInfo SITL::var_info2[] = { // vibration frequencies on each axis AP_GROUPINFO("BAUDLIMIT_EN", 28, SITL, telem_baudlimit_enable, 0), + // @Group: PLD_ + // @Path: ./SIM_Precland.cpp + AP_SUBGROUPINFO(precland_sim, "PLD_", 29, SITL, SIM_Precland), + AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 77cf6b325f..29c6b6ab9c 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -8,6 +8,7 @@ #include "SIM_Gripper_Servo.h" #include "SIM_Gripper_EPM.h" #include "SIM_Parachute.h" +#include "SIM_Precland.h" class AP_Logger; @@ -249,6 +250,7 @@ public: Gripper_EPM gripper_epm_sim; Parachute parachute_sim; + SIM_Precland precland_sim; }; } // namespace SITL