forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 18
/
events.pde
130 lines (114 loc) · 3.27 KB
/
events.pde
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
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void failsafe_short_on_event(int16_t fstype)
{
// This is how to handle a short loss of control signal failsafe.
failsafe = fstype;
ch3_failsafe_timer = millis();
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on, "));
switch(control_mode)
{
case MANUAL:
case STABILIZE:
case FLY_BY_WIRE_A:
case FLY_BY_WIRE_B:
case TRAINING:
set_mode(CIRCLE);
break;
case AUTO:
case GUIDED:
case LOITER:
if(g.short_fs_action == 1) {
set_mode(RTL);
}
break;
case CIRCLE:
case RTL:
default:
break;
}
gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
}
static void failsafe_long_on_event(int16_t fstype)
{
// This is how to handle a long loss of control signal failsafe.
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on, "));
// If the GCS is locked up we allow control to revert to RC
hal.rcin->clear_overrides();
failsafe = fstype;
switch(control_mode)
{
case MANUAL:
case STABILIZE:
case FLY_BY_WIRE_A:
case FLY_BY_WIRE_B:
case TRAINING:
case CIRCLE:
set_mode(RTL);
break;
case AUTO:
case GUIDED:
case LOITER:
if(g.long_fs_action == 1) {
set_mode(RTL);
}
break;
case RTL:
default:
break;
}
gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
}
static void failsafe_short_off_event()
{
// We're back in radio contact
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event off"));
failsafe = FAILSAFE_NONE;
// re-read the switch so we can return to our preferred mode
// --------------------------------------------------------
if (control_mode == CIRCLE ||
(g.short_fs_action == 1 && control_mode == RTL)) {
reset_control_switch();
}
}
void low_battery_event(void)
{
if (battery.low_batttery) {
return;
}
gcs_send_text_fmt(PSTR("Low Battery %.2fV Used %.0f mAh"),
battery.voltage, battery.current_total_mah);
set_mode(RTL);
g.throttle_cruise = THROTTLE_CRUISE;
battery.low_batttery = true;
}
////////////////////////////////////////////////////////////////////////////////
// repeating event control
/*
update state for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
*/
static void update_events(void)
{
if (event_state.repeat == 0 || (millis() - event_state.start_time_ms) < event_state.delay_ms) {
return;
}
// event_repeat = -1 means repeat forever
if (event_state.repeat != 0) {
event_state.start_time_ms = millis();
switch (event_state.type) {
case EVENT_TYPE_SERVO:
hal.rcout->enable_ch(event_state.rc_channel);
if (event_state.repeat & 1) {
servo_write(event_state.rc_channel, event_state.undo_value);
} else {
servo_write(event_state.rc_channel, event_state.servo_value);
}
break;
case EVENT_TYPE_RELAY:
relay.toggle();
break;
}
if (event_state.repeat > 0) {
event_state.repeat--;
}
}
}