-
Notifications
You must be signed in to change notification settings - Fork 17k
/
fence.cpp
76 lines (63 loc) · 2.89 KB
/
fence.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
#include "Copter.h"
// Code to integrate AC_Fence library with main ArduCopter code
#if AC_FENCE == ENABLED
// fence_check - ask fence library to check for breaches and initiate the response
// called at 1hz
void Copter::fence_check()
{
// ignore any fence activity when not armed
if(!motors->armed()) {
return;
}
const uint8_t orig_breaches = fence.get_breaches();
// check for new breaches; new_breaches is bitmask of fence types breached
const uint8_t new_breaches = fence.check();
// if there is a new breach take action
if (new_breaches) {
// if the user wants some kind of response and motors are armed
if(fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY ) {
// disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle
// don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
if (ap.land_complete || (flightmode->has_manual_throttle() && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){
init_disarm_motors();
}else{
// if we are within 100m of the fence, RTL
if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
if (!set_mode(RTL, MODE_REASON_FENCE_BREACH)) {
set_mode(LAND, MODE_REASON_FENCE_BREACH);
}
}else{
// if more than 100m outside the fence just force a land
set_mode(LAND, MODE_REASON_FENCE_BREACH);
}
}
}
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, new_breaches);
} else if (orig_breaches) {
// record clearing of breach
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, ERROR_CODE_ERROR_RESOLVED);
}
}
// fence_send_mavlink_status - send fence status to ground station
void Copter::fence_send_mavlink_status(mavlink_channel_t chan)
{
if (fence.enabled()) {
// traslate fence library breach types to mavlink breach types
uint8_t mavlink_breach_type = FENCE_BREACH_NONE;
uint8_t breaches = fence.get_breaches();
if ((breaches & AC_FENCE_TYPE_ALT_MAX) != 0) {
mavlink_breach_type = FENCE_BREACH_MAXALT;
}
if ((breaches & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) != 0) {
mavlink_breach_type = FENCE_BREACH_BOUNDARY;
}
// send status
mavlink_msg_fence_status_send(chan,
(int8_t)(fence.get_breaches()!=0),
fence.get_breach_count(),
mavlink_breach_type,
fence.get_breach_time());
}
}
#endif