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

@@ -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<int16_t>(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());
+    }
 }

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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include "SIM_Precland.h"
+#include "AP_HAL/AP_HAL.h"
+#include "AP_Math/AP_Math.h"
+#include "AP_Common/Location.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 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<int32_t>(_origin_lat * 1.0e7f),
+            static_cast<int32_t>(_origin_lon * 1.0e7f),
+            static_cast<int32_t>(_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;
+    }
+}

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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+#include "stdint.h"
+#include <AP_Param/AP_Param.h>
+#include <AP_Math/AP_Math.h>
+
+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<bool>(_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;
+};
+
+}

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
 };
     

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