Line data Source code
1 : #include "AC_Fence.h"
2 :
3 : #if AP_FENCE_ENABLED
4 :
5 : #include <AP_Vehicle/AP_Vehicle_Type.h>
6 :
7 : #ifndef AC_FENCE_DUMMY_METHODS_ENABLED
8 : #define AC_FENCE_DUMMY_METHODS_ENABLED (!(APM_BUILD_TYPE(APM_BUILD_Rover) | APM_BUILD_COPTER_OR_HELI | APM_BUILD_TYPE(APM_BUILD_ArduPlane) | APM_BUILD_TYPE(APM_BUILD_ArduSub) | (AP_FENCE_ENABLED == 1)))
9 : #endif
10 :
11 : #if !AC_FENCE_DUMMY_METHODS_ENABLED
12 :
13 : #include <AP_AHRS/AP_AHRS.h>
14 : #include <AP_HAL/AP_HAL.h>
15 : #include <AP_Logger/AP_Logger.h>
16 : #include <GCS_MAVLink/GCS.h>
17 : #include <AP_Terrain/AP_Terrain.h>
18 :
19 : extern const AP_HAL::HAL& hal;
20 :
21 : #if APM_BUILD_TYPE(APM_BUILD_Rover)
22 : #define AC_FENCE_TYPE_DEFAULT AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON
23 : #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
24 : #define AC_FENCE_TYPE_DEFAULT AC_FENCE_TYPE_POLYGON
25 : #else
26 : #define AC_FENCE_TYPE_DEFAULT AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON
27 : #endif
28 :
29 : // default boundaries
30 : #define AC_FENCE_ALT_MAX_DEFAULT 100.0f // default max altitude is 100m
31 : #define AC_FENCE_ALT_MIN_DEFAULT -10.0f // default maximum depth in meters
32 : #define AC_FENCE_CIRCLE_RADIUS_DEFAULT 300.0f // default circular fence radius is 300m
33 : #define AC_FENCE_ALT_MAX_BACKUP_DISTANCE 20.0f // after fence is broken we recreate the fence 20m further up
34 : #define AC_FENCE_ALT_MIN_BACKUP_DISTANCE 20.0f // after fence is broken we recreate the fence 20m further down
35 : #define AC_FENCE_MARGIN_DEFAULT 2.0f // default distance in meters that autopilot's should maintain from the fence to avoid a breach
36 : #define AC_FENCE_MANUAL_RECOVERY_TIME_MIN 10000 // pilot has 10seconds to recover during which time the autopilot will not attempt to re-take control
37 :
38 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
39 : #define AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE 100.0 // after fence is broken we recreate the fence 100m further out
40 : #define AC_FENCE_OPTIONS_DEFAULT OPTIONS::DISABLE_MODE_CHANGE
41 : #else
42 : #define AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE 20.0 // after fence is broken we recreate the fence 20m further out
43 : #define AC_FENCE_OPTIONS_DEFAULT 0
44 : #endif
45 :
46 : #define AC_FENCE_NOTIFY_DEFAULT 1.0f // default to notifications at 1Hz
47 :
48 : //#define AC_FENCE_DEBUG
49 :
50 : const AP_Param::GroupInfo AC_Fence::var_info[] = {
51 :
52 : // @Param: ENABLE
53 : // @DisplayName: Fence enable/disable
54 : // @Description: Allows you to enable (1) or disable (0) the fence functionality. Fences can still be enabled and disabled via mavlink or an RC option, but these changes are not persisted.
55 : // @Values: 0:Disabled,1:Enabled
56 : // @User: Standard
57 : AP_GROUPINFO("ENABLE", 0, AC_Fence, _enabled, 0),
58 :
59 : // @Param: TYPE
60 : // @DisplayName: Fence Type
61 : // @Description: Configured fence types held as bitmask. Max altitude, Circle and Polygon fences will be immediately enabled if configured. Min altitude fence will only be enabled once the minimum altitude is reached.
62 : // @Bitmask{Rover}: 1:Circle Centered on Home,2:Inclusion/Exclusion Circles+Polygons
63 : // @Bitmask{Copter, Plane, Sub}: 0:Max altitude,1:Circle Centered on Home,2:Inclusion/Exclusion Circles+Polygons,3:Min altitude
64 : // @User: Standard
65 : AP_GROUPINFO("TYPE", 1, AC_Fence, _configured_fences, AC_FENCE_TYPE_DEFAULT),
66 :
67 : // @Param: ACTION
68 : // @DisplayName: Fence Action
69 : // @Description: What action should be taken when fence is breached
70 : // @Values{Copter}: 0:Report Only,1:RTL or Land,2:Always Land,3:SmartRTL or RTL or Land,4:Brake or Land,5:SmartRTL or Land
71 : // @Values{Rover}: 0:Report Only,1:RTL or Hold,2:Hold,3:SmartRTL or RTL or Hold,4:SmartRTL or Hold,5:Terminate,6:Loiter or Hold
72 : // @Values{Plane}: 0:Report Only,1:RTL,6:Guided,7:GuidedThrottlePass,8:AUTOLAND if possible else RTL
73 : // @Values: 0:Report Only,1:RTL or Land
74 : // @User: Standard
75 : AP_GROUPINFO("ACTION", 2, AC_Fence, _action, float(Action::RTL_AND_LAND)),
76 :
77 : // @Param{Copter, Plane, Sub}: ALT_MAX
78 : // @DisplayName: Fence Maximum Altitude
79 : // @Description: Maximum altitude allowed before geofence triggers. See FENCE_ALT_MAX_TP for reference frame.
80 : // @Units: m
81 : // @Range: 10 1000
82 : // @Increment: 1
83 : // @User: Standard
84 : AP_GROUPINFO_FRAME("ALT_MAX", 3, AC_Fence, _alt_max_m, AC_FENCE_ALT_MAX_DEFAULT, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_SUB | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_PLANE),
85 :
86 : // @Param: RADIUS
87 : // @DisplayName: Circular Fence Radius
88 : // @Description: Circle fence radius which when breached will cause an RTL
89 : // @Units: m
90 : // @Range: 30 10000
91 : // @User: Standard
92 : AP_GROUPINFO("RADIUS", 4, AC_Fence, _circle_radius_m, AC_FENCE_CIRCLE_RADIUS_DEFAULT),
93 :
94 : // @Param: MARGIN
95 : // @DisplayName: Fence Margin
96 : // @Description: Distance that autopilot's should maintain from the fence to avoid a breach
97 : // @Units: m
98 : // @Range: 1 10
99 : // @User: Standard
100 : AP_GROUPINFO("MARGIN", 5, AC_Fence, _margin_m, AC_FENCE_MARGIN_DEFAULT),
101 :
102 : // @Param: TOTAL
103 : // @DisplayName: Fence polygon point total
104 : // @Description: Number of polygon points saved in eeprom (do not update manually)
105 : // @Range: 1 20
106 : // @User: Standard
107 : AP_GROUPINFO("TOTAL", 6, AC_Fence, _total, 0),
108 :
109 : // @Param{Copter, Plane, Sub}: ALT_MIN
110 : // @DisplayName: Fence Minimum Altitude
111 : // @Description: Minimum altitude allowed before geofence triggers. See FENCE_ALT_MIN_TP for reference frame.
112 : // @Units: m
113 : // @Range: -100 100
114 : // @Increment: 1
115 : // @User: Standard
116 : AP_GROUPINFO_FRAME("ALT_MIN", 7, AC_Fence, _alt_min_m, AC_FENCE_ALT_MIN_DEFAULT, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_SUB | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_PLANE),
117 :
118 : // @Param{Plane}: RET_RALLY
119 : // @DisplayName: Fence Return to Rally
120 : // @Description: Should the vehicle return to fence return point or rally point
121 : // @Values: 0:Fence Return Point,1:Nearest Rally Point
122 : // @Range: 0 1
123 : // @Increment: 1
124 : // @User: Standard
125 : AP_GROUPINFO_FRAME("RET_RALLY", 8, AC_Fence, _ret_rally, 0, AP_PARAM_FRAME_PLANE),
126 :
127 : // @Param{Plane}: RET_ALT
128 : // @DisplayName: Fence Return Altitude
129 : // @Description: Altitude the vehicle will transit to when a fence breach occurs
130 : // @Units: m
131 : // @Range: 0 32767
132 : // @Increment: 1
133 : // @User: Standard
134 : AP_GROUPINFO_FRAME("RET_ALT", 9, AC_Fence, _ret_altitude, 0.0f, AP_PARAM_FRAME_PLANE),
135 :
136 : // @Param{Plane, Copter}: AUTOENABLE
137 : // @DisplayName: Fence Auto-Enable
138 : // @Description: Auto-enable of fences. AutoEnableOnTakeoff enables all configured fences, except the minimum altitude fence (which is enabled when the minimum altitude is reached), after autotakeoffs reach altitude. During autolandings the fences will be disabled. AutoEnableDisableFloorOnLanding enables all configured fences, except the minimum altitude fence (which is enabled when the minimum altitude is reached), after autotakeoffs reach altitude. During autolandings only the Minimum Altitude fence will be disabled. AutoEnableOnlyWhenArmed enables all configured fences on arming, except the minimum altitude fence (which is enabled when the minimum altitude is reached), but no fences are disabled during autolandings. However, fence breaches are ignored while executing prior breach recovery actions which may include autolandings.
139 : // @Values{Plane, Copter}: 0:AutoEnableOff,1:AutoEnableOnTakeoff,2:AutoEnableDisableFloorOnLanding,3:AutoEnableOnlyWhenArmed
140 : // @Range: 0 3
141 : // @Increment: 1
142 : // @User: Standard
143 : AP_GROUPINFO_FRAME("AUTOENABLE", 10, AC_Fence, _auto_enabled, static_cast<uint8_t>(AutoEnable::ALWAYS_DISABLED), AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI),
144 :
145 : // @Param: OPTIONS
146 : // @DisplayName: Fence options
147 : // @Description: When bit 0 is set disable mode change following fence action until fence breach is cleared. When bit 1 is set the allowable flight areas is the union of all polygon and circle fence areas instead of the intersection, which means a fence breach occurs only if you are outside all of the fence areas.
148 : // @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas, 2:Notify on margin breaches
149 : // @User: Standard
150 : AP_GROUPINFO("OPTIONS", 11, AC_Fence, _options, static_cast<uint16_t>(AC_FENCE_OPTIONS_DEFAULT)),
151 :
152 : // @Param: NTF_FREQ
153 : // @DisplayName: Fence margin notification frequency in hz
154 : // @Description: When bit 2 of FENCE_OPTIONS is set this parameter controls the frequency of margin breach notifications. If set to 0 only new margin breaches are notified.
155 : // @Range: 0 10
156 : // @Units: Hz
157 : // @User: Advanced
158 : AP_GROUPINFO("NTF_FREQ", 12, AC_Fence, _notify_freq, static_cast<float>(AC_FENCE_NOTIFY_DEFAULT)),
159 :
160 : // @Param: MARGIN_XY
161 : // @DisplayName: Fence Horizontal Margin
162 : // @Description: Distance that autopilot's should maintain from the fence in the horizontal plane to avoid a breach. If set to 0 then FENCE_MARGIN is used.
163 : // @Units: m
164 : // @Range: 0 50
165 : // @User: Standard
166 : AP_GROUPINFO("MARGIN_XY", 13, AC_Fence, _margin_ne_m, 0),
167 :
168 : // @Param: ALT_MAX_TP
169 : // @DisplayName: Altitude max frame type
170 : // @Description: The altitude reference frame for the maximum altitude fence. Can be AMSL, home-relative, origin relative or AGL.
171 : // @User: Advanced
172 : // @Values: 0:Above sea level, 1:Above Home, 2: Above Origin, 3: Above Terrain
173 : AP_GROUPINFO_FRAME("ALT_MAX_TP", 14, AC_Fence, _alt_max_type, float(Location::AltFrame::ABOVE_HOME), AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_SUB | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_PLANE),
174 :
175 : // @Param: ALT_MIN_TP
176 : // @DisplayName: Altitude min frame type
177 : // @Description: The altitude reference frame for the minimum altitude fence. Can be AMSL, home-relative, origin relative or AGL.
178 : // @User: Advanced
179 : // @Values: 0:Above sea level, 1:Above Home, 2: Above Origin, 3: Above Terrain
180 : AP_GROUPINFO_FRAME("ALT_MIN_TP", 15, AC_Fence, _alt_min_type, float(Location::AltFrame::ABOVE_HOME), AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_SUB | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_PLANE),
181 :
182 : AP_GROUPEND
183 : };
184 :
185 : /// Default constructor.
186 2061 : AC_Fence::AC_Fence()
187 : {
188 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
189 2061 : if (_singleton != nullptr) {
190 0 : AP_HAL::panic("Fence must be singleton");
191 : }
192 : #endif
193 2061 : _singleton = this;
194 2061 : AP_Param::setup_object_defaults(this, var_info);
195 2061 : if (_enabled) {
196 0 : _enabled_fences = _configured_fences.get() & ~AC_FENCE_TYPE_ALT_MIN;
197 : }
198 2061 : }
199 :
200 : // get a user-friendly list of fences
201 494 : void AC_Fence::get_fence_names(uint8_t fences, ExpandingString& msg)
202 : {
203 494 : if (!fences) {
204 0 : return;
205 : }
206 : static const char* FENCE_NAMES[] = {
207 : "Max Alt",
208 : "Circle",
209 : "Polygon",
210 : "Min Alt",
211 : };
212 494 : uint8_t i = 0;
213 494 : uint8_t nfences = 0;
214 2019 : while (fences !=0) {
215 1525 : if (fences & 0x1) {
216 514 : if (nfences > 0) {
217 20 : if (!(fences & ~1U)) {
218 18 : msg.printf(" and ");
219 : } else {
220 2 : msg.printf(", ");
221 : }
222 : }
223 514 : msg.printf("%s", FENCE_NAMES[i]);
224 514 : nfences++;
225 : }
226 1525 : fences >>= 1;
227 1525 : i++;
228 : }
229 494 : msg.printf(" fence");
230 494 : if (nfences>1) {
231 18 : msg.printf("s");
232 : }
233 : }
234 :
235 : // print a message about the passed in fences
236 153 : void AC_Fence::print_fence_message(const char* message, uint8_t fences) const
237 : {
238 153 : if (!fences) {
239 3 : return;
240 : }
241 :
242 : char msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
243 150 : ExpandingString e(msg, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
244 150 : AC_Fence::get_fence_names(fences, e);
245 150 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "%s %s", e.get_writeable_string(), message);
246 150 : }
247 :
248 : // should be called @10Hz to handle loading from eeprom
249 1486795 : void AC_Fence::update()
250 : {
251 1486795 : _poly_loader.update();
252 : // if someone changes the parameter we want to enable or disable everything
253 1486795 : if (_enabled != _last_enabled || _auto_enabled != _last_auto_enabled) {
254 : // reset the auto mask since we just reconfigured all of fencing
255 141 : _last_enabled = _enabled;
256 141 : _last_auto_enabled = _auto_enabled;
257 141 : if (_enabled) {
258 73 : _enabled_fences = _configured_fences.get() & ~AC_FENCE_TYPE_ALT_MIN;
259 : } else {
260 68 : _enabled_fences = 0;
261 : }
262 : }
263 : #ifdef AC_FENCE_DEBUG
264 : static uint32_t last_msg_count = 0;
265 : if (get_enabled_fences() && last_msg_count++ % 10 == 0) {
266 : print_fence_message("active", get_enabled_fences());
267 : print_fence_message("breached", get_breaches());
268 : }
269 : #endif
270 1486795 : }
271 :
272 : // enable or disable configured fences present in fence_types
273 : // also updates the _min_alt_state enum if update_auto_enable is true
274 : // returns a bitmask of fences that were changed
275 158453 : uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_enable)
276 : {
277 158453 : uint8_t fences = _configured_fences.get() & fence_types;
278 158453 : uint8_t enabled_fences = _enabled_fences;
279 158453 : if (value) {
280 96 : enabled_fences |= fences;
281 : } else {
282 158357 : enabled_fences &= ~fences;
283 : }
284 :
285 158453 : if (update_auto_enable && (fences & AC_FENCE_TYPE_ALT_MIN) != 0) {
286 : // remember that min-alt fence was manually enabled/disabled
287 47 : _min_alt_state = value ? MinAltState::MANUALLY_ENABLED : MinAltState::MANUALLY_DISABLED;
288 : }
289 :
290 158453 : uint8_t fences_to_change = _enabled_fences ^ enabled_fences;
291 :
292 158453 : if (!fences_to_change) {
293 158330 : return 0;
294 : }
295 :
296 : #if HAL_LOGGING_ENABLED
297 123 : AP::logger().Write_Event(value ? LogEvent::FENCE_ENABLE : LogEvent::FENCE_DISABLE);
298 123 : if (fences_to_change & AC_FENCE_TYPE_ALT_MAX) {
299 18 : AP::logger().Write_Event(value ? LogEvent::FENCE_ALT_MAX_ENABLE : LogEvent::FENCE_ALT_MAX_DISABLE);
300 : }
301 123 : if (fences_to_change & AC_FENCE_TYPE_CIRCLE) {
302 30 : AP::logger().Write_Event(value ? LogEvent::FENCE_CIRCLE_ENABLE : LogEvent::FENCE_CIRCLE_DISABLE);
303 : }
304 123 : if (fences_to_change & AC_FENCE_TYPE_ALT_MIN) {
305 65 : AP::logger().Write_Event(value ? LogEvent::FENCE_ALT_MIN_ENABLE : LogEvent::FENCE_ALT_MIN_DISABLE);
306 : }
307 123 : if (fences_to_change & AC_FENCE_TYPE_POLYGON) {
308 49 : AP::logger().Write_Event(value ? LogEvent::FENCE_POLYGON_ENABLE : LogEvent::FENCE_POLYGON_DISABLE);
309 : }
310 : #endif
311 :
312 123 : _enabled_fences = enabled_fences;
313 :
314 123 : if (!value) {
315 51 : clear_breach(fences_to_change);
316 51 : if (update_auto_enable) {
317 : // explicit user disable resets the manual-recovery suppression
318 : // window so that a fresh fence enable will check immediately
319 38 : _manual_recovery_start_ms = 0;
320 : }
321 : }
322 :
323 123 : return fences_to_change;
324 : }
325 :
326 : /// enable/disable fence floor only
327 0 : void AC_Fence::enable_floor()
328 : {
329 0 : enable(true, AC_FENCE_TYPE_ALT_MIN);
330 0 : }
331 :
332 0 : void AC_Fence::disable_floor()
333 : {
334 0 : enable(false, AC_FENCE_TYPE_ALT_MIN);
335 0 : }
336 :
337 : /*
338 : called on arming
339 : */
340 1127 : void AC_Fence::auto_enable_fence_on_arming(void)
341 : {
342 1127 : if (auto_enabled() != AC_Fence::AutoEnable::ONLY_WHEN_ARMED) {
343 1123 : return;
344 : }
345 :
346 : // reset min alt state, after an auto-enable the min alt fence can be auto-enabled on
347 : // reaching altitude
348 4 : _min_alt_state = MinAltState::DEFAULT;
349 :
350 4 : const uint8_t fences = enable(true, AC_FENCE_ARMING_FENCES, false);
351 4 : print_fence_message("auto-enabled", fences);
352 : }
353 :
354 : /*
355 : called on disarming
356 : */
357 1103 : void AC_Fence::auto_disable_fence_on_disarming(void)
358 : {
359 1103 : if (auto_enabled() != AC_Fence::AutoEnable::ONLY_WHEN_ARMED) {
360 1099 : return;
361 : }
362 :
363 4 : const uint8_t fences = enable(false, AC_FENCE_ALL_FENCES, false);
364 4 : print_fence_message("auto-disabled", fences);
365 : }
366 :
367 : /*
368 : called when an auto-takeoff is complete
369 : */
370 290 : void AC_Fence::auto_enable_fence_after_takeoff(void)
371 : {
372 579 : if (auto_enabled() != AutoEnable::ENABLE_ON_AUTO_TAKEOFF &&
373 289 : auto_enabled() != AutoEnable::ENABLE_DISABLE_FLOOR_ONLY) {
374 285 : return;
375 : }
376 :
377 : // reset min-alt state
378 5 : _min_alt_state = MinAltState::DEFAULT;
379 :
380 5 : const uint8_t fences = enable(true, AC_FENCE_ALL_FENCES, false);
381 5 : print_fence_message("auto-enabled", fences);
382 : }
383 :
384 : // return fences that should be auto-disabled when requested
385 1097236 : uint8_t AC_Fence::get_auto_disable_fences(void) const
386 : {
387 1097236 : uint8_t auto_disable = 0;
388 1097236 : switch (auto_enabled()) {
389 2605 : case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF:
390 2605 : auto_disable = AC_FENCE_ALL_FENCES;
391 2605 : break;
392 1094631 : case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
393 : case AC_Fence::AutoEnable::ONLY_WHEN_ARMED:
394 : default: // when auto disable is not set we still need to disable the altmin fence on landing
395 1094631 : auto_disable = AC_FENCE_TYPE_ALT_MIN;
396 1094631 : break;
397 : }
398 1097236 : if (_min_alt_state == MinAltState::MANUALLY_ENABLED) {
399 : // don't auto-disable min alt fence if manually enabled
400 3789 : auto_disable &= ~AC_FENCE_TYPE_ALT_MIN;
401 : }
402 1097236 : return auto_disable;
403 : }
404 :
405 11588490 : uint8_t AC_Fence::present() const
406 : {
407 11588490 : uint8_t mask = AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_ALT_MIN | AC_FENCE_TYPE_ALT_MAX;
408 11588490 : if (_poly_loader.total_fence_count() > 0) {
409 831219 : mask |= AC_FENCE_TYPE_POLYGON;
410 : }
411 :
412 11588490 : return _configured_fences.get() & mask;
413 : }
414 :
415 : /// get_enabled_fences - returns bitmask of enabled fences
416 8227814 : uint8_t AC_Fence::get_enabled_fences() const
417 : {
418 8227814 : return _enabled_fences & present();
419 : }
420 :
421 : /// get_margin_ne_m - returns the fence margin in meters
422 580468 : float AC_Fence::get_margin_ne_m() const
423 : {
424 580468 : return is_positive(_margin_ne_m.get()) ? _margin_ne_m.get() : _margin_m.get();
425 : }
426 :
427 : // additional checks for the polygon fence:
428 2149 : bool AC_Fence::pre_arm_check_polygon(char *failure_msg, const uint8_t failure_msg_len) const
429 : {
430 2149 : if (!(_configured_fences & AC_FENCE_TYPE_POLYGON)) {
431 : // not enabled; all good
432 1136 : return true;
433 : }
434 :
435 1013 : if (! _poly_loader.loaded()) {
436 139 : hal.util->snprintf(failure_msg, failure_msg_len, "Polygon fence(s) invalid");
437 139 : return false;
438 : }
439 :
440 874 : if (!_poly_loader.check_inclusion_circle_margin(get_margin_ne_m())) {
441 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Polygon fence margin is greater than inclusion circle radius");
442 0 : return false;
443 : }
444 :
445 874 : return true;
446 : }
447 :
448 : // additional checks for the circle fence:
449 2010 : bool AC_Fence::pre_arm_check_circle(char *failure_msg, const uint8_t failure_msg_len) const
450 : {
451 2010 : if (_circle_radius_m < 0) {
452 1 : hal.util->snprintf(failure_msg, failure_msg_len, "Invalid Circle FENCE_RADIUS value");
453 1 : return false;
454 : }
455 2009 : if (_circle_radius_m < get_margin_ne_m()) {
456 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Circle FENCE_MARGIN is greater than FENCE_RADIUS");
457 0 : return false;
458 : }
459 :
460 2009 : return true;
461 : }
462 :
463 : // additional checks for the alt fence:
464 2009 : bool AC_Fence::pre_arm_check_alt(char *failure_msg, const uint8_t failure_msg_len) const
465 : {
466 2009 : if (_alt_max_m < 0.0f) {
467 1 : hal.util->snprintf(failure_msg, failure_msg_len, "Invalid FENCE_ALT_MAX value");
468 1 : return false;
469 : }
470 :
471 2008 : if (_alt_min_m < -100.0f) {
472 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Invalid FENCE_ALT_MIN value");
473 0 : return false;
474 : }
475 2008 : return true;
476 : }
477 :
478 128868 : bool AC_Fence::terrain_database_required() const
479 : {
480 128868 : return _alt_min_type == Location::AltFrame::ABOVE_TERRAIN || _alt_max_type == Location::AltFrame::ABOVE_TERRAIN;
481 : }
482 :
483 : /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully
484 138793 : bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const
485 : {
486 : // if fences are enabled but none selected fail pre-arm check
487 138793 : if (_enabled && !present()) {
488 45 : hal.util->snprintf(failure_msg, failure_msg_len, "Fences enabled, but none selected");
489 45 : return false;
490 : }
491 :
492 : // if AUTOENABLE = 1 or 2 warn now, but fail in a later release
493 : // PARAMETER_CONVERSION - Added: Jul-2024 for ArduPilot-4.6
494 138748 : if (_auto_enabled == 1 || _auto_enabled == 2) {
495 : static uint32_t last_autoenable_warn_ms;
496 155 : const uint32_t now_ms = AP_HAL::millis();
497 155 : if (now_ms - last_autoenable_warn_ms > 60000) {
498 6 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FENCE_AUTOENABLE is %u, will be removed in 4.7, use 3", unsigned(_auto_enabled));
499 6 : last_autoenable_warn_ms = now_ms;
500 : }
501 : }
502 :
503 : // if not enabled or not fence set-up always return true
504 138748 : if ((!enabled() && !_auto_enabled) || !_configured_fences) {
505 136430 : return true;
506 : }
507 :
508 : // if we have horizontal limits enabled, check we can get a
509 : // relative position from the AHRS
510 3253 : if ((_configured_fences & AC_FENCE_TYPE_CIRCLE) ||
511 935 : (_configured_fences & AC_FENCE_TYPE_POLYGON)) {
512 1533 : Vector2f position;
513 1533 : if (!AP::ahrs().get_relative_position_NE_home(position)) {
514 169 : hal.util->snprintf(failure_msg, failure_msg_len, "Fence requires position");
515 169 : return false;
516 : }
517 : }
518 :
519 2149 : if (!pre_arm_check_polygon(failure_msg, failure_msg_len)) {
520 139 : return false;
521 : }
522 :
523 2010 : if (!pre_arm_check_circle(failure_msg, failure_msg_len)) {
524 1 : return false;
525 : }
526 :
527 2009 : if (!pre_arm_check_alt(failure_msg, failure_msg_len)) {
528 1 : return false;
529 : }
530 :
531 2008 : auto breached_fences = _breached_fences;
532 2008 : if (auto_enabled() == AC_Fence::AutoEnable::ONLY_WHEN_ARMED) {
533 217 : Location loc;
534 217 : if (!AP::ahrs().get_location(loc)) {
535 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Fence requires position");
536 0 : return false;
537 : }
538 217 : if (_poly_loader.breached(loc)) {
539 0 : breached_fences |= AC_FENCE_TYPE_POLYGON;
540 : }
541 : }
542 :
543 : // check no limits are currently breached
544 2008 : if (breached_fences) {
545 : char msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
546 324 : ExpandingString e(msg, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
547 324 : AC_Fence::get_fence_names(_breached_fences, e);
548 324 : hal.util->snprintf(failure_msg, failure_msg_len, "Vehicle breaching %s", e.get_writeable_string());
549 324 : return false;
550 324 : }
551 :
552 : // validate FENCE_MARGIN parameter range
553 1684 : if (_margin_m < 0.0f) {
554 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Invalid FENCE_MARGIN value");
555 0 : return false;
556 : }
557 :
558 1684 : if (_margin_ne_m < 0.0f) {
559 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Invalid FENCE_MARGIN_XY value");
560 0 : return false;
561 : }
562 :
563 : // change min-alt into the same frame as max-alt to make them comparable:
564 1684 : const Location::AltFrame alt_max_frame = get_alt_max_frame();
565 1684 : float framed_alt_min_m = _alt_min_m;
566 1684 : if (get_alt_min_frame() != alt_max_frame) {
567 29 : Location loc;
568 29 : if (!AP::ahrs().get_location(loc)) {
569 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Fence requires position");
570 0 : return false;
571 : }
572 29 : loc.set_alt_m(_alt_min_m, get_alt_min_frame());
573 29 : if (!loc.get_alt_m(alt_max_frame, framed_alt_min_m)) {
574 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Fence requires terrain");
575 0 : return false;
576 : }
577 : }
578 :
579 1684 : if (_alt_max_m < framed_alt_min_m) {
580 2 : hal.util->snprintf(failure_msg, failure_msg_len, "FENCE_ALT_MAX < FENCE_ALT_MIN");
581 2 : return false;
582 : }
583 :
584 1682 : if (_alt_max_m - framed_alt_min_m <= 2.0f * _margin_m) {
585 0 : hal.util->snprintf(failure_msg, failure_msg_len, "FENCE_MARGIN too big");
586 0 : return false;
587 : }
588 :
589 : // if we got this far everything must be ok
590 1682 : return true;
591 : }
592 :
593 : /*
594 : get our altitude in the supplied frame
595 : return false if not available
596 : */
597 257133 : bool AC_Fence::get_alt_in_frame_m(Location::AltFrame alt_frame, float &alt) const
598 : {
599 257133 : const auto &ahrs = AP::ahrs();
600 :
601 257133 : if (alt_frame == Location::AltFrame::ABOVE_ORIGIN) {
602 : // try and get it relative origin
603 17357 : if (!ahrs.get_relative_position_D_origin_float(alt)) {
604 : // treat as a breach if origin not known
605 34 : return false;
606 : }
607 17323 : alt = -alt;
608 17323 : return true;
609 : }
610 :
611 239776 : if (alt_frame == Location::AltFrame::ABOVE_HOME) {
612 : // try and get it relative origin
613 220600 : ahrs.get_relative_position_D_home(alt);
614 220600 : alt = -alt;
615 220600 : return true;
616 : }
617 :
618 19176 : Location loc;
619 19176 : if (AP::ahrs().get_location(loc)) {
620 19065 : if (loc.get_alt_m(alt_frame, alt)) {
621 19065 : return true;
622 : }
623 : }
624 :
625 111 : return false;
626 : }
627 :
628 : // update safe alt min - home *must* be set before this function is called with absolute reference frames
629 27574 : void AC_Fence::update_safe_alt_min()
630 : {
631 : // update safe alt min, failure to get home will have already breached above
632 27574 : if (_alt_min_type == Location::AltFrame::ABSOLUTE) {
633 : float home_alt;
634 4075 : UNUSED_RESULT(AP::ahrs().get_home().get_alt_m(Location::AltFrame::ABSOLUTE, home_alt));
635 4075 : _safe_relhome_alt_min_m = _alt_min_m - home_alt - _margin_m;
636 : } else {
637 23499 : _safe_relhome_alt_min_m = _alt_min_m - _margin_m;
638 : }
639 27574 : }
640 :
641 : /// returns true if we have freshly breached the maximum altitude
642 : /// fence; also may set up a fallback fence which, if breached, will
643 : /// cause the altitude fence to be freshly breached
644 155704 : bool AC_Fence::check_fence_alt_max()
645 : {
646 : // altitude fence check
647 155704 : if (!(get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX)) {
648 : // not enabled; no breach
649 90939 : return false;
650 : }
651 :
652 : float _curr_alt_u_m;
653 64765 : if (!get_alt_in_alt_max_frame_m(_curr_alt_u_m)) {
654 : // if we can't get the alt then it is a breach
655 145 : return true;
656 : }
657 :
658 : // record distance above/below breach
659 64620 : _alt_max_breach_distance_m = _curr_alt_u_m - _alt_max_m;
660 :
661 : // update safe alt max, failure to get home will have already breached above
662 64620 : if (_alt_max_type == Location::AltFrame::ABSOLUTE) {
663 : float home_alt;
664 5113 : UNUSED_RESULT(AP::ahrs().get_home().get_alt_m(Location::AltFrame::ABSOLUTE, home_alt));
665 5113 : _safe_relhome_alt_max_m = _alt_max_m - home_alt - _margin_m;
666 : } else {
667 59507 : _safe_relhome_alt_max_m = _alt_max_m - _margin_m;
668 : }
669 :
670 : // check if we are over the altitude fence
671 64620 : if (_curr_alt_u_m >= _alt_max_m) {
672 : // check for a new breach or a breach of the backup fence
673 8876 : if (!(_breached_fences & AC_FENCE_TYPE_ALT_MAX) ||
674 4419 : (!is_zero(_alt_max_backup_m) && _curr_alt_u_m >= _alt_max_backup_m)) {
675 :
676 : // new breach
677 40 : record_breach(AC_FENCE_TYPE_ALT_MAX);
678 :
679 : // create a backup fence 20m higher up
680 40 : _alt_max_backup_m = _curr_alt_u_m + AC_FENCE_ALT_MAX_BACKUP_DISTANCE;
681 : // new breach
682 40 : return true;
683 : }
684 : // old breach
685 4417 : return false;
686 60163 : } else if (_curr_alt_u_m >= get_safe_alt_max_m()) {
687 1886 : record_margin_breach(AC_FENCE_TYPE_ALT_MAX);
688 : } else {
689 58277 : clear_margin_breach(AC_FENCE_TYPE_ALT_MAX);
690 : }
691 :
692 : // not breached
693 :
694 : // clear max alt breach if present
695 60163 : if ((_breached_fences & AC_FENCE_TYPE_ALT_MAX) != 0) {
696 35 : clear_breach(AC_FENCE_TYPE_ALT_MAX);
697 35 : _alt_max_backup_m = 0.0f;
698 : }
699 :
700 60163 : return false;
701 : }
702 :
703 : /// returns true if we have freshly breached the minimum altitude
704 : /// fence; also may set up a fallback fence which, if breached, will
705 : /// cause the altitude fence to be freshly breached
706 94796 : bool AC_Fence::check_fence_alt_min()
707 : {
708 : // altitude fence check
709 94796 : if (!(get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN)) {
710 : // not enabled; no breach
711 72590 : return false;
712 : }
713 :
714 : float _curr_alt_u_m;
715 22206 : if (!get_alt_in_alt_min_frame_m(_curr_alt_u_m)) {
716 : // if we can't get the alt then it is a breach
717 0 : return true;
718 : }
719 :
720 : // record distance above/below breach
721 22206 : _alt_min_breach_distance_m = _alt_min_m - _curr_alt_u_m;
722 :
723 : // update safe alt min, failure to get home will have already breached above
724 22206 : update_safe_alt_min();
725 :
726 : // check if we are under the altitude fence
727 22206 : if (_curr_alt_u_m <= _alt_min_m) {
728 :
729 :
730 : // check for a new breach or a breach of the backup fence
731 11199 : if (!(_breached_fences & AC_FENCE_TYPE_ALT_MIN) ||
732 5580 : (!is_zero(_alt_min_backup_m) && _curr_alt_u_m <= _alt_min_backup_m)) {
733 :
734 : // new breach
735 42 : record_breach(AC_FENCE_TYPE_ALT_MIN);
736 :
737 : // create a backup fence 20m lower down
738 42 : _alt_min_backup_m = _curr_alt_u_m - AC_FENCE_ALT_MIN_BACKUP_DISTANCE;
739 : // new breach
740 42 : return true;
741 : }
742 : // old breach
743 5577 : return false;
744 16587 : } else if (_curr_alt_u_m <= get_safe_alt_min_m()) {
745 2465 : record_margin_breach(AC_FENCE_TYPE_ALT_MIN);
746 : } else {
747 14122 : clear_margin_breach(AC_FENCE_TYPE_ALT_MIN);
748 : }
749 :
750 : // not breached
751 :
752 : // clear min alt breach if present
753 16587 : if ((_breached_fences & AC_FENCE_TYPE_ALT_MIN) != 0) {
754 14 : clear_breach(AC_FENCE_TYPE_ALT_MIN);
755 14 : _alt_min_backup_m = 0.0f;
756 : }
757 :
758 16587 : return false;
759 : }
760 :
761 :
762 : /// auto enable fence floor
763 94796 : bool AC_Fence::auto_enable_fence_floor()
764 : {
765 : // altitude fence check
766 94796 : if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) // not configured
767 34135 : || (get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) // already enabled
768 11929 : || _min_alt_state == MinAltState::MANUALLY_DISABLED // user has manually disabled the fence
769 129179 : || (!_enabled && (auto_enabled() == AC_Fence::AutoEnable::ALWAYS_DISABLED
770 248 : || auto_enabled() == AutoEnable::ENABLE_ON_AUTO_TAKEOFF))) {
771 : // not enabled
772 89428 : return false;
773 : }
774 :
775 : float _curr_alt_u_m;
776 5368 : if (!get_alt_in_alt_min_frame_m(_curr_alt_u_m)) {
777 : // if we can't get the alt then don't enable yet
778 0 : return true;
779 : }
780 :
781 : // update safe alt min, failure to get home will have already breached above
782 5368 : update_safe_alt_min();
783 :
784 : // check if we are over the altitude fence
785 5368 : if (!floor_enabled() && _curr_alt_u_m >= get_safe_alt_min_m()) {
786 22 : enable(true, AC_FENCE_TYPE_ALT_MIN, false);
787 22 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Min Alt fence enabled (auto enable)");
788 22 : return true;
789 : }
790 :
791 5346 : return false;
792 : }
793 :
794 : // check_fence_polygon - returns true if the poly fence is freshly
795 : // breached. That includes being inside exclusion zones and outside
796 : // inclusions zones
797 155704 : bool AC_Fence::check_fence_polygon()
798 : {
799 155704 : if (!(get_enabled_fences() & AC_FENCE_TYPE_POLYGON)) {
800 : // not enabled; no breach
801 120248 : clear_breach(AC_FENCE_TYPE_POLYGON);
802 120248 : return false;
803 : }
804 :
805 35456 : const bool was_breached = _breached_fences & AC_FENCE_TYPE_POLYGON;
806 :
807 35456 : if (_last_fence_check_loc_valid && _poly_loader.breached(_last_fence_check_loc, _polygon_breach_distance_m, _polygon_nearest_point)) {
808 3890 : if (!was_breached) {
809 40 : record_breach(AC_FENCE_TYPE_POLYGON);
810 40 : return true;
811 : }
812 3850 : return false;
813 31566 : } else if (_polygon_breach_distance_m < 0 && fabsf(_polygon_breach_distance_m) < get_margin_ne_m()) {
814 7027 : record_margin_breach(AC_FENCE_TYPE_POLYGON);
815 : } else {
816 24539 : clear_margin_breach(AC_FENCE_TYPE_POLYGON);
817 : }
818 :
819 31566 : if (was_breached) {
820 21 : clear_breach(AC_FENCE_TYPE_POLYGON);
821 : }
822 31566 : return false;
823 : }
824 :
825 : /// check_fence_circle - returns true if the circle fence (defined via
826 : /// parameters) has been freshly breached. May also set up a backup
827 : /// fence outside the fence and return a fresh breach if that backup
828 : /// fence is breached.
829 155704 : bool AC_Fence::check_fence_circle()
830 : {
831 155704 : if (!(get_enabled_fences() & AC_FENCE_TYPE_CIRCLE)) {
832 : // not enabled; no breach
833 110372 : return false;
834 : }
835 :
836 45332 : Vector2f home;
837 45332 : if (AP::ahrs().get_relative_position_NE_home(home)) {
838 : // we (may) remain breached if we can't update home
839 43442 : _home_distance_m = home.length();
840 :
841 43442 : if (is_zero(home.length_squared())) {
842 6438 : _circle_breach_direction.x = _circle_radius_m;
843 6438 : _circle_breach_direction.y = 0.0f;
844 : } else {
845 37004 : _circle_breach_direction = home.normalized() * _circle_radius_m.get() - home;
846 : }
847 : }
848 :
849 : // record distance outside/inside the fence
850 45332 : _circle_breach_distance_m = _home_distance_m - _circle_radius_m;
851 :
852 : // check if we are outside the fence
853 45332 : if (_home_distance_m >= _circle_radius_m) {
854 :
855 : // check for a new breach or a breach of the backup fence
856 2035 : if (!(_breached_fences & AC_FENCE_TYPE_CIRCLE) ||
857 1015 : (!is_zero(_circle_radius_backup_m) && _home_distance_m >= _circle_radius_backup_m)) {
858 : // new breach
859 : // create a backup fence 20m or 100m further out
860 9 : record_breach(AC_FENCE_TYPE_CIRCLE);
861 9 : _circle_radius_backup_m = _home_distance_m + AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE;
862 9 : return true;
863 : }
864 1011 : return false;
865 44312 : } else if (_home_distance_m >= _circle_radius_m - get_margin_ne_m()) {
866 3151 : record_margin_breach(AC_FENCE_TYPE_CIRCLE);
867 : } else {
868 41161 : clear_margin_breach(AC_FENCE_TYPE_CIRCLE);
869 : }
870 :
871 : // not currently breached
872 :
873 : // clear circle breach if present
874 44312 : if (_breached_fences & AC_FENCE_TYPE_CIRCLE) {
875 5 : clear_breach(AC_FENCE_TYPE_CIRCLE);
876 5 : _circle_radius_backup_m = 0.0f;
877 : }
878 :
879 44312 : return false;
880 : }
881 :
882 :
883 : /// check - returns bitmask of fence types breached (if any)
884 2201372 : uint8_t AC_Fence::check(bool disable_auto_fences)
885 : {
886 2201372 : uint8_t ret = 0;
887 2201372 : uint8_t disabled_fences = disable_auto_fences ? get_auto_disable_fences() : 0;
888 2201372 : uint8_t fences_to_disable = disabled_fences & _enabled_fences;
889 :
890 : // clear any breach from a non-enabled fence
891 2201372 : clear_breach(~_configured_fences);
892 : // clear any breach from disabled fences
893 2201372 : clear_breach(fences_to_disable);
894 : // clear any margin breach from a non-enabled fence
895 2201372 : clear_margin_breach(~_configured_fences);
896 : // clear any marginbreach from disabled fences
897 2201372 : clear_margin_breach(fences_to_disable);
898 :
899 2201372 : if (_min_alt_state == MinAltState::MANUALLY_ENABLED) {
900 : // if user has manually enabled the min-alt fence then don't auto-disable
901 13674 : fences_to_disable &= ~AC_FENCE_TYPE_ALT_MIN;
902 : }
903 :
904 : // report on any fences that were auto-disabled
905 2201372 : if (fences_to_disable) {
906 60 : print_fence_message("auto-disabled", fences_to_disable);
907 : }
908 :
909 : #if 0
910 : /*
911 : this debug log message is very useful both when developing tests
912 : and doing manual SITL fence testing
913 : */
914 : {
915 : float _curr_alt_d_m;
916 : AP::ahrs().get_relative_position_D_home(_curr_alt_d_m);
917 :
918 : // @LoggerMessage: FENC
919 : // @Description: Fence status - development diagnostic message
920 : // @Field: TimeUS: Time since system startup
921 : // @Field: EN: bitmask of enabled fences
922 : // @Field: AE: bitmask of automatically enabled fences
923 : // @Field: CF: bitmask of configured-in-parameters fences
924 : // @Field: EF: bitmask of enabled fences
925 : // @Field: DF: bitmask of currently disabled fences
926 : // @Field: Alt: current vehicle altitude
927 : AP::logger().WriteStreaming("FENC", "TimeUS,EN,AE,CF,EF,DF,Alt", "QIIIIIf",
928 : AP_HAL::micros64(),
929 : enabled(),
930 : _auto_enabled,
931 : _configured_fences,
932 : get_enabled_fences(),
933 : disabled_fences,
934 : _curr_alt_d_m*-1);
935 : }
936 : #endif
937 :
938 : // return immediately if disabled
939 2201372 : if ((!enabled() && !_auto_enabled && !(_configured_fences & AC_FENCE_TYPE_ALT_MIN)) || !_configured_fences) {
940 2043060 : return 0;
941 : }
942 :
943 : // take lock inside critical section
944 158312 : if (!_poly_loader.get_loaded_fence_semaphore().take_nonblocking()) {
945 3 : return 0;
946 : }
947 :
948 : // disable the (temporarily) disabled fences
949 158309 : enable(false, disabled_fences, false);
950 :
951 : // update the location at the time the check was made
952 158309 : _last_fence_check_loc_valid = AP::ahrs().get_location(_last_fence_check_loc);
953 :
954 : // maximum altitude fence check
955 158309 : if (!(disabled_fences & AC_FENCE_TYPE_ALT_MAX) && check_fence_alt_max()) {
956 185 : ret |= AC_FENCE_TYPE_ALT_MAX;
957 : }
958 :
959 : // minimum altitude fence check, do this before auto-disabling (e.g. because falling)
960 : // so that any action can be taken
961 158309 : if (!(disabled_fences & AC_FENCE_TYPE_ALT_MIN) && check_fence_alt_min()) {
962 42 : ret |= AC_FENCE_TYPE_ALT_MIN;
963 : }
964 :
965 : // auto enable floor unless auto enable on auto takeoff has been set (which means other behaviour is required)
966 158309 : if (!(disabled_fences & AC_FENCE_TYPE_ALT_MIN)) {
967 94796 : auto_enable_fence_floor();
968 : }
969 :
970 : // circle fence check
971 158309 : if (!(disabled_fences & AC_FENCE_TYPE_CIRCLE) && check_fence_circle()) {
972 9 : ret |= AC_FENCE_TYPE_CIRCLE;
973 : }
974 :
975 : // polygon fence check
976 158309 : if (!(disabled_fences & AC_FENCE_TYPE_POLYGON) && check_fence_polygon()) {
977 40 : ret |= AC_FENCE_TYPE_POLYGON;
978 : }
979 :
980 : // check if pilot is attempting to recover manually
981 : // this is done last so that _breached_fences is correct
982 158309 : if (_manual_recovery_start_ms != 0) {
983 : // we ignore any fence breaches during the manual recovery period which is about 10 seconds
984 6965 : if ((AP_HAL::millis() - _manual_recovery_start_ms) < AC_FENCE_MANUAL_RECOVERY_TIME_MIN) {
985 6917 : _poly_loader.get_loaded_fence_semaphore().give();
986 6917 : return 0;
987 : }
988 : // recovery period has passed so reset manual recovery time
989 : // and continue with fence breach checks
990 48 : _manual_recovery_start_ms = 0;
991 : }
992 :
993 151392 : _poly_loader.get_loaded_fence_semaphore().give();
994 :
995 : // return any new breaches that have occurred
996 151392 : return ret;
997 : }
998 :
999 : // returns true if the destination is within fence (used to reject waypoints outside the fence)
1000 2138 : bool AC_Fence::check_location_within_fence(const Location& loc)
1001 : {
1002 : // Altitude fence check - Fence Ceiling
1003 2138 : if ((get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX)) {
1004 : float alt_m;
1005 0 : if (loc.get_alt_m(_alt_max_type, alt_m)) {
1006 0 : if (alt_m > _alt_max_m) {
1007 0 : return false;
1008 : }
1009 : }
1010 : }
1011 :
1012 : // Altitude fence check - Fence Floor
1013 2138 : if ((get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN)) {
1014 : float alt_m;
1015 6 : if (loc.get_alt_m(_alt_min_type, alt_m)) {
1016 6 : if (alt_m < _alt_min_m) {
1017 0 : return false;
1018 : }
1019 : }
1020 : }
1021 :
1022 : // Circular fence check
1023 2138 : if ((get_enabled_fences() & AC_FENCE_TYPE_CIRCLE)) {
1024 7 : if (AP::ahrs().get_home().get_distance(loc) > _circle_radius_m) {
1025 3 : return false;
1026 : }
1027 : }
1028 :
1029 : // polygon fence check
1030 2135 : if ((get_enabled_fences() & AC_FENCE_TYPE_POLYGON)) {
1031 1 : if (_poly_loader.breached(loc)) {
1032 0 : return false;
1033 : }
1034 : }
1035 :
1036 2135 : return true;
1037 : }
1038 :
1039 : /// record_breach - update breach bitmask, time and count
1040 131 : void AC_Fence::record_breach(uint8_t fence_type)
1041 : {
1042 : // if we haven't already breached a limit, update the breach time
1043 131 : if (!_breached_fences) {
1044 122 : const uint32_t now = AP_HAL::millis();
1045 122 : _breach_time = now;
1046 :
1047 : // emit a message indicated we're newly-breached, but not too often
1048 122 : if (now - _last_breach_notify_sent_ms > 1000) {
1049 120 : _last_breach_notify_sent_ms = now;
1050 120 : GCS_SEND_MESSAGE(MSG_FENCE_STATUS);
1051 : }
1052 : }
1053 :
1054 : // update breach count
1055 131 : if (_breach_count < 65500) {
1056 131 : _breach_count++;
1057 : }
1058 :
1059 : // update bitmask
1060 131 : _breached_fences |= fence_type;
1061 131 : }
1062 :
1063 : /// record_margin_breach - update margin_breach bitmask, time and count
1064 14529 : void AC_Fence::record_margin_breach(uint8_t fence_type)
1065 : {
1066 : // if we haven't already breached a margin limit, update the breach time
1067 14529 : const uint32_t now = AP_HAL::millis();
1068 :
1069 14529 : bool notify_margin_breach = false;
1070 :
1071 14529 : if (option_enabled(OPTIONS::NOTIFY_MARGIN_BREACH)) {
1072 540 : if ((!is_zero(_notify_freq)
1073 540 : && AP_HAL::timeout_expired(_last_margin_breach_notify_sent_ms, now,
1074 540 : uint32_t(roundf(1000.0f / _notify_freq))))
1075 1080 : || !(_breached_fence_margins & fence_type)) {
1076 20 : notify_margin_breach = true;
1077 : }
1078 : }
1079 :
1080 14529 : if (!(_breached_fence_margins & fence_type)) {
1081 171 : _margin_breach_time = now;
1082 : }
1083 :
1084 : // update bitmask
1085 14529 : _breached_fence_margins |= fence_type;
1086 :
1087 : // emit a message indicated we're newly-breached, but not too often
1088 14529 : if (notify_margin_breach) {
1089 20 : _last_margin_breach_notify_sent_ms = now;
1090 : char msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
1091 20 : ExpandingString e(msg, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
1092 20 : AC_Fence::get_fence_names(_breached_fence_margins, e);
1093 20 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "%s in %.1fm", e.get_writeable_string(), fabsf(get_breach_distance(_breached_fence_margins)));
1094 20 : }
1095 14529 : }
1096 :
1097 : /// clear_breach - update breach bitmask
1098 4523118 : void AC_Fence::clear_breach(uint8_t fence_type)
1099 : {
1100 4523118 : _breached_fences &= ~fence_type;
1101 4523118 : }
1102 :
1103 : /// clear_margin_breach - update margin reach bitmask
1104 4540843 : void AC_Fence::clear_margin_breach(uint8_t fence_type)
1105 : {
1106 4540843 : if (_breached_fence_margins & fence_type) {
1107 165 : if (option_enabled(OPTIONS::NOTIFY_MARGIN_BREACH)) {
1108 2 : print_fence_message("cleared margin breach", fence_type);
1109 : }
1110 : }
1111 :
1112 4540843 : _breached_fence_margins &= ~fence_type;
1113 4540843 : }
1114 :
1115 : /// get_breach_distance - returns maximum distance in meters outside
1116 : /// negative values mean distance in meters inside
1117 : /// of the given fences. fence_type is a bitmask here.
1118 65 : float AC_Fence::get_breach_distance(uint8_t fence_type) const
1119 : {
1120 65 : fence_type = fence_type & present();
1121 65 : float max_m = -FLT_MAX;
1122 :
1123 65 : if (fence_type & AC_FENCE_TYPE_ALT_MAX) {
1124 13 : max_m = MAX(_alt_max_breach_distance_m, max_m);
1125 : }
1126 65 : if (fence_type & AC_FENCE_TYPE_ALT_MIN) {
1127 16 : max_m = MAX(_alt_min_breach_distance_m, max_m);
1128 : }
1129 65 : if (fence_type & AC_FENCE_TYPE_CIRCLE) {
1130 15 : max_m = MAX(_circle_breach_distance_m, max_m);
1131 : }
1132 65 : if (fence_type & AC_FENCE_TYPE_POLYGON) {
1133 33 : max_m = MAX(_polygon_breach_distance_m, max_m);
1134 : }
1135 65 : return max_m;
1136 : }
1137 :
1138 : /// get_breach_direction - returns direction to the closest fence breach in NED frame.
1139 : /// fence_check_pos is the absolute position when the check was made.
1140 : /// fence_type is a bitmask here.
1141 0 : bool AC_Fence::get_breach_direction_NED(uint8_t fence_type, Vector3f& direction_to_closest, Location& fence_check_pos) const
1142 : {
1143 0 : fence_type = fence_type & present();
1144 0 : if (_last_fence_check_loc_valid) {
1145 0 : fence_check_pos = _last_fence_check_loc;
1146 : }
1147 0 : float closest_m = FLT_MAX;
1148 :
1149 0 : if (fence_type & AC_FENCE_TYPE_ALT_MAX && fabsf(_alt_max_breach_distance_m) < closest_m) {
1150 0 : direction_to_closest = Vector3f{0, 0, _alt_max_breach_distance_m};
1151 0 : closest_m = fabsf(_alt_max_breach_distance_m);
1152 : }
1153 0 : if (fence_type & AC_FENCE_TYPE_ALT_MIN && fabsf(_alt_min_breach_distance_m) < closest_m) {
1154 0 : direction_to_closest = Vector3f{0, 0, -_alt_min_breach_distance_m};
1155 0 : closest_m = fabsf(_alt_min_breach_distance_m);
1156 : }
1157 0 : if (fence_type & AC_FENCE_TYPE_CIRCLE && fabsf(_circle_breach_distance_m) < closest_m) {
1158 0 : direction_to_closest = Vector3f{_circle_breach_direction.x, _circle_breach_direction.y, 0};
1159 0 : closest_m = fabsf(_circle_breach_distance_m);
1160 : }
1161 0 : if (fence_type & AC_FENCE_TYPE_POLYGON && fabsf(_polygon_breach_distance_m) < closest_m) {
1162 0 : direction_to_closest = Vector3f{_polygon_nearest_point.x, _polygon_nearest_point.y, 0};
1163 0 : closest_m = fabsf(_polygon_breach_distance_m);
1164 : }
1165 0 : return _last_fence_check_loc_valid;
1166 : }
1167 :
1168 : /// manual_recovery_start - caller indicates that pilot is re-taking manual control so fence should be disabled for 10 seconds
1169 : /// has no effect if no breaches have occurred
1170 3709 : void AC_Fence::manual_recovery_start()
1171 : {
1172 : // return immediate if we haven't breached a fence
1173 3709 : if (!_breached_fences) {
1174 3624 : return;
1175 : }
1176 :
1177 : // Not armed so return
1178 85 : if (!hal.util->get_soft_armed()) {
1179 2 : return;
1180 : }
1181 :
1182 : // record time pilot began manual recovery
1183 83 : _manual_recovery_start_ms = AP_HAL::millis();
1184 :
1185 83 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Manual recovery started");
1186 : }
1187 :
1188 : // methods for mavlink SYS_STATUS message (send_sys_status)
1189 3358005 : bool AC_Fence::sys_status_present() const
1190 : {
1191 3358005 : return present();
1192 : }
1193 :
1194 1119335 : bool AC_Fence::sys_status_enabled() const
1195 : {
1196 1119335 : if (!sys_status_present()) {
1197 498842 : return false;
1198 : }
1199 620493 : if (_action == Action::REPORT_ONLY) {
1200 5887 : return false;
1201 : }
1202 : // Fence is only enabled when the flag is enabled
1203 614606 : return enabled();
1204 : }
1205 :
1206 1119335 : bool AC_Fence::sys_status_failed() const
1207 : {
1208 1119335 : if (!sys_status_present()) {
1209 : // not failed if not present; can fail if present but not enabled
1210 498842 : return false;
1211 : }
1212 620493 : if (get_breaches() != 0) {
1213 7948 : return true;
1214 : }
1215 612545 : return false;
1216 : }
1217 :
1218 3798185 : AC_PolyFence_loader &AC_Fence::polyfence()
1219 : {
1220 3798185 : return _poly_loader;
1221 : }
1222 4975242 : const AC_PolyFence_loader &AC_Fence::polyfence() const
1223 : {
1224 4975242 : return _poly_loader;
1225 : }
1226 :
1227 :
1228 : #else // build type is not appropriate; provide a dummy implementation:
1229 : const AP_Param::GroupInfo AC_Fence::var_info[] = { AP_GROUPEND };
1230 :
1231 37 : AC_Fence::AC_Fence() {};
1232 :
1233 0 : uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_enable) { return 0; }
1234 :
1235 0 : void AC_Fence::enable_floor() {}
1236 0 : void AC_Fence::disable_floor() {}
1237 17294 : void AC_Fence::update() {}
1238 :
1239 0 : void AC_Fence::auto_enable_fence_after_takeoff() {}
1240 0 : void AC_Fence::auto_enable_fence_on_arming() {}
1241 0 : void AC_Fence::auto_disable_fence_on_disarming() {}
1242 :
1243 0 : uint8_t AC_Fence::present() const { return 0; }
1244 :
1245 0 : uint8_t AC_Fence::get_enabled_fences() const { return 0; }
1246 :
1247 0 : bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const { return true; }
1248 0 : bool AC_Fence::terrain_database_required() const { return false; }
1249 0 : uint8_t AC_Fence::check(bool disable_auto_fences) { return 0; }
1250 0 : bool AC_Fence::check_location_within_fence(const Location& loc) { return true; }
1251 0 : float AC_Fence::get_breach_distance(uint8_t fence_type) const { return 0.0; }
1252 0 : bool AC_Fence::get_breach_direction_NED(uint8_t fence_type, Vector3f& direction, Location& fence_check_pos) const { return false; }
1253 0 : void AC_Fence::get_fence_names(uint8_t fences, ExpandingString& msg) { }
1254 0 : void AC_Fence::print_fence_message(const char* msg, uint8_t fences) const {}
1255 :
1256 0 : void AC_Fence::manual_recovery_start() {}
1257 :
1258 0 : bool AC_Fence::sys_status_present() const { return false; }
1259 0 : bool AC_Fence::sys_status_enabled() const { return false; }
1260 0 : bool AC_Fence::sys_status_failed() const { return false; }
1261 :
1262 0 : AC_PolyFence_loader &AC_Fence::polyfence()
1263 : {
1264 0 : return _poly_loader;
1265 : }
1266 0 : const AC_PolyFence_loader &AC_Fence::polyfence() const
1267 : {
1268 0 : return _poly_loader;
1269 : }
1270 :
1271 : #endif // #if AC_FENCE_DUMMY_METHODS_ENABLED
1272 :
1273 : // singleton instance
1274 : AC_Fence *AC_Fence::_singleton;
1275 :
1276 : namespace AP
1277 : {
1278 :
1279 16388059 : AC_Fence *fence()
1280 : {
1281 16388059 : return AC_Fence::get_singleton();
1282 : }
1283 :
1284 : }
1285 :
1286 : #endif // AP_FENCE_ENABLED
|
