Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include "SIM_config.h"
17 :
18 : #include "SIM_Precland.h"
19 : #include "AP_HAL/AP_HAL.h"
20 : #include "AP_Math/AP_Math.h"
21 : #include "AP_Common/Location.h"
22 : #include "SITL.h"
23 : #include <stdio.h>
24 : #include <GCS_MAVLink/GCS.h>
25 :
26 : using namespace SITL;
27 :
28 : // table of user settable parameters
29 : const AP_Param::GroupInfo SIM_Precland::var_info[] = {
30 :
31 : // @Param: ENABLE
32 : // @DisplayName: Preland device Sim enable/disable
33 : // @Description: Allows you to enable (1) or disable (0) the Preland simulation
34 : // @Values: 0:Disabled,1:Enabled
35 : // @User: Advanced
36 : AP_GROUPINFO("ENABLE", 0, SIM_Precland, _enable, 0),
37 :
38 : // @Param: LAT
39 : // @DisplayName: Precland device center's latitude
40 : // @Description: Precland device center's latitude
41 : // @Units: deg
42 : // @Increment: 0.000001
43 : // @Range: -90 90
44 : // @User: Advanced
45 : AP_GROUPINFO("LAT", 1, SIM_Precland, _device_lat, 0),
46 :
47 : // @Param: LON
48 : // @DisplayName: Precland device center's longitude
49 : // @Description: Precland device center's longitude
50 : // @Units: deg
51 : // @Increment: 0.000001
52 : // @Range: -180 180
53 : // @User: Advanced
54 : AP_GROUPINFO("LON", 2, SIM_Precland, _device_lon, 0),
55 :
56 : // @Param: HEIGHT
57 : // @DisplayName: Precland device center's height SITL origin
58 : // @Description: Precland device center's height above SITL origin. Assumes a 2x2m square as station base
59 : // @Units: m
60 : // @Increment: 1
61 : // @Range: 0 10000
62 : // @User: Advanced
63 : AP_GROUPINFO("HEIGHT", 3, SIM_Precland, _device_height, 0),
64 :
65 : // @Param: YAW
66 : // @DisplayName: Precland device systems rotation from north
67 : // @Description: Precland device systems rotation from north
68 : // @Units: deg
69 : // @Increment: 1
70 : // @Range: -180 +180
71 : // @User: Advanced
72 : AP_GROUPINFO("YAW", 4, SIM_Precland, _orient_yaw, 0),
73 :
74 : // @Param: RATE
75 : // @DisplayName: Precland device update rate
76 : // @Description: Precland device rate. e.g led patter refresh rate, RF message rate, etc.
77 : // @Units: Hz
78 : // @Range: 0 200
79 : // @User: Advanced
80 : AP_GROUPINFO("RATE", 5, SIM_Precland, _rate, 100),
81 :
82 : // @Param: TYPE
83 : // @DisplayName: Precland device radiance type
84 : // @Description: Precland device radiance type: it can be a cylinder, a cone, or a sphere.
85 : // @Values: 0:cylinder,1:cone,2:sphere
86 : // @User: Advanced
87 : AP_GROUPINFO("TYPE", 6, SIM_Precland, _type, SIM_Precland::PRECLAND_TYPE_CYLINDER),
88 :
89 : // @Param: ALT_LMT
90 : // @DisplayName: Precland device alt range
91 : // @Description: Precland device maximum range altitude
92 : // @Units: m
93 : // @Range: 0 100
94 : // @User: Advanced
95 : AP_GROUPINFO("ALT_LMT", 7, SIM_Precland, _alt_limit, 15),
96 :
97 : // @Param: DIST_LMT
98 : // @DisplayName: Precland device lateral range
99 : // @Description: Precland device maximum lateral range
100 : // @Units: m
101 : // @Range: 5 100
102 : // @User: Advanced
103 : AP_GROUPINFO("DIST_LMT", 8, SIM_Precland, _dist_limit, 10),
104 :
105 : // @Param: ORIENT
106 : // @DisplayName: Precland device orientation
107 : // @Description: Precland device orientation vector
108 : // @Values: 0:Front, 4:Back, 24:Up
109 : // @User: Advanced
110 : AP_GROUPINFO("ORIENT", 9, SIM_Precland, _orient, ROTATION_PITCH_90),
111 :
112 : // @Param: OPTIONS
113 : // @DisplayName: SIM_Precland extra options
114 : // @Description: SIM_Precland extra options
115 : // @Bitmask: 0: Enable target distance
116 : // @User: Advanced
117 : AP_GROUPINFO("OPTIONS", 10, SIM_Precland, _options, 0),
118 :
119 : #if AP_SIM_SHIP_ENABLED
120 : // @Param: SHIP
121 : // @DisplayName: SIM_Precland follow ship
122 : // @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
123 : // @Values: 0:Disabled,1:Enabled
124 : // @User: Advanced
125 : AP_GROUPINFO("SHIP", 11, SIM_Precland, _ship, 0),
126 : #endif
127 :
128 : AP_GROUPEND
129 : };
130 :
131 946580 : void SIM_Precland::update(const Location &loc)
132 : {
133 946580 : if (!_enable) {
134 0 : _healthy = false;
135 912863 : return;
136 : }
137 946580 : if (is_zero(_alt_limit) || _dist_limit < 1.0f) {
138 0 : _healthy = false;
139 0 : return;
140 : }
141 :
142 1893160 : Location device_center(static_cast<int32_t>(_device_lat * 1.0e7f),
143 1893160 : static_cast<int32_t>(_device_lon * 1.0e7f),
144 946580 : static_cast<int32_t>(_device_height*100),
145 946580 : Location::AltFrame::ABOVE_ORIGIN);
146 :
147 946580 : if (device_center.lat == 0 && device_center.lng == 0 && device_center.alt == 0) {
148 7185 : const uint32_t now_ms = AP_HAL::millis();
149 7185 : if (now_ms - last_set_parameters_warning_ms > 5000) {
150 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Set SIM_PLD_LAT, SIM_PLD_LAT and SIM_PLD_ALT");
151 2 : last_set_parameters_warning_ms = now_ms;
152 : }
153 7185 : _healthy = false;
154 7185 : return;
155 : }
156 :
157 : #if AP_SIM_SHIP_ENABLED
158 939395 : if (_ship == 1) {
159 : /*
160 : make precland target follow the simulated ship if the ship is enabled
161 : */
162 0 : auto *sitl = AP::sitl();
163 0 : Location shiploc;
164 0 : if (sitl != nullptr && sitl->models.shipsim.get_location(shiploc) && !shiploc.is_zero()) {
165 0 : shiploc.change_alt_frame(Location::AltFrame::ABOVE_ORIGIN);
166 0 : device_center = shiploc;
167 : }
168 : }
169 : #endif
170 :
171 : // axis of cone or cylinder inside which the vehicle receives signals from simulated precland device
172 939395 : Vector3d axis{1, 0, 0};
173 939395 : axis.rotate((Rotation)_orient.get()); // unit vector in direction of axis of cone or cylinder
174 :
175 939395 : device_center.change_alt_frame(loc.get_alt_frame());
176 :
177 939395 : Vector3d position_wrt_device = device_center.get_distance_NED_double(loc); // position of vehicle with respect to preland device center
178 :
179 : // longitudinal distance of vehicle from the precland device
180 : // this is the distance of vehicle from the plane which is passing through precland device center and perpendicular to axis of cone/cylinder
181 : // this plane is the ground plane when the axis has PITCH_90 rotation
182 939395 : Vector3d projection_on_axis = position_wrt_device.projected(axis);
183 939395 : const float longitudinal_dist = projection_on_axis.length();
184 :
185 : // lateral distance of vehicle from the precland device
186 : // this is the perpendicular distance of vehicle from the axis of cone/cylinder
187 939395 : const float lateral_distance = safe_sqrt(MAX(0, position_wrt_device.length_squared() - longitudinal_dist*longitudinal_dist));
188 :
189 : // sign of projection's dot product with axis tells if vehicle is in front of beacon
190 : // return false if vehicle if vehicle is longitudinally too far away from precland device
191 : // for PITCH_90 orientation, longitudinal distance = alt of vehicle - device_height (in m)
192 939395 : if (projection_on_axis.dot(axis) <= 0 || longitudinal_dist > _alt_limit) {
193 515621 : _healthy = false;
194 515621 : return;
195 : }
196 :
197 423774 : const uint32_t now = AP_HAL::millis();
198 423774 : if (now - _last_update_ms < 1000.0f * (1.0f / _rate)) {
199 388470 : return;
200 : }
201 35304 : _last_update_ms = now;
202 :
203 35304 : switch (_type) {
204 14656 : case PRECLAND_TYPE_CONE: {
205 : // lateral_limit is the limit of how far the vehicle can laterally be from precland_device
206 : // in case of cone, this limit increases gradually as the vehicle moves longitudinally far away from precland device
207 14656 : const float lateral_limit = longitudinal_dist * _dist_limit / _alt_limit;
208 14656 : if (lateral_distance > lateral_limit) {
209 1546 : _healthy = false;
210 1546 : return;
211 : }
212 13110 : break;
213 : }
214 4882 : case PRECLAND_TYPE_SPHERE: {
215 4882 : if (position_wrt_device.length() > _dist_limit) {
216 2 : _healthy = false;
217 2 : return;
218 : }
219 4880 : break;
220 : }
221 15766 : default:
222 : case PRECLAND_TYPE_CYLINDER: {
223 15766 : if (lateral_distance > _dist_limit) {
224 39 : _healthy = false;
225 39 : return;
226 : }
227 15727 : break;
228 : }
229 : }
230 33717 : _target_pos = position_wrt_device;
231 33717 : _healthy = true;
232 : }
233 :
234 2094 : void SIM_Precland::set_default_location(float lat, float lon, int16_t yaw) {
235 2094 : if (is_zero(_device_lat) && is_zero(_device_lon)) {
236 2094 : _device_lat.set(lat);
237 2094 : _device_lon.set(lon);
238 2094 : _orient_yaw.set(yaw);
239 : }
240 2094 : }
|
