Line data Source code
1 : #include "Plane.h"
2 :
3 : // returns true if the vehicle is in landing sequence. Intended only
4 : // for use in failsafe code.
5 3 : bool Plane::failsafe_in_landing_sequence() const
6 : {
7 3 : if (flight_stage == AP_FixedWing::FlightStage::LAND) {
8 0 : return true;
9 : }
10 : #if HAL_QUADPLANE_ENABLED
11 3 : if (quadplane.in_vtol_land_sequence()) {
12 0 : return true;
13 : }
14 : #endif
15 3 : if (mission.get_in_landing_sequence_flag()) {
16 0 : return true;
17 : }
18 3 : return false;
19 : }
20 :
21 5 : void Plane::rc_failsafe_short_on_event()
22 : {
23 : // This is how to handle a short loss of control signal failsafe.
24 5 : failsafe.state = FAILSAFE_SHORT;
25 5 : failsafe.saved_mode_number = control_mode->mode_number();
26 5 : switch (control_mode->mode_number())
27 : {
28 5 : case Mode::Number::MANUAL:
29 : case Mode::Number::STABILIZE:
30 : case Mode::Number::ACRO:
31 : case Mode::Number::FLY_BY_WIRE_A:
32 : case Mode::Number::AUTOTUNE:
33 : case Mode::Number::FLY_BY_WIRE_B:
34 : case Mode::Number::CRUISE:
35 : case Mode::Number::TRAINING:
36 5 : if(plane.emergency_landing) {
37 0 : set_mode(mode_fbwa, ModeReason::RADIO_FAILSAFE); // emergency landing switch overrides normal action to allow out of range landing
38 0 : break;
39 : }
40 5 : if(g.fs_action_short == FS_ACTION_SHORT_FBWA) {
41 0 : set_mode(mode_fbwa, ModeReason::RADIO_FAILSAFE);
42 5 : } else if (g.fs_action_short == FS_ACTION_SHORT_FBWB) {
43 0 : set_mode(mode_fbwb, ModeReason::RADIO_FAILSAFE);
44 : } else {
45 5 : set_mode(mode_circle, ModeReason::RADIO_FAILSAFE); // circle if action = 0 or 1
46 : }
47 5 : break;
48 :
49 : #if HAL_QUADPLANE_ENABLED
50 0 : case Mode::Number::QSTABILIZE:
51 : case Mode::Number::QLOITER:
52 : case Mode::Number::QHOVER:
53 : #if QAUTOTUNE_ENABLED
54 : case Mode::Number::QAUTOTUNE:
55 : #endif
56 : case Mode::Number::QACRO:
57 0 : if (quadplane.option_is_set(QuadPlane::Option::FS_RTL)) {
58 0 : set_mode(mode_rtl, ModeReason::RADIO_FAILSAFE);
59 0 : } else if (quadplane.option_is_set(QuadPlane::Option::FS_QRTL)) {
60 0 : set_mode(mode_qrtl, ModeReason::RADIO_FAILSAFE);
61 : } else {
62 0 : set_mode(mode_qland, ModeReason::RADIO_FAILSAFE);
63 : }
64 0 : break;
65 : #endif // HAL_QUADPLANE_ENABLED
66 :
67 0 : case Mode::Number::AUTO:
68 : #if MODE_AUTOLAND_ENABLED
69 : case Mode::Number::AUTOLAND:
70 : #endif
71 : {
72 0 : if (failsafe_in_landing_sequence()) {
73 : // don't failsafe in a landing sequence
74 0 : break;
75 : }
76 : FALLTHROUGH;
77 : }
78 : case Mode::Number::AVOID_ADSB:
79 : case Mode::Number::GUIDED:
80 : case Mode::Number::LOITER:
81 : case Mode::Number::THERMAL:
82 0 : if (g.fs_action_short != FS_ACTION_SHORT_BESTGUESS) { // if acton = 0(BESTGUESS) this group of modes take no action
83 0 : failsafe.saved_mode_number = control_mode->mode_number();
84 0 : if (g.fs_action_short == FS_ACTION_SHORT_FBWA) {
85 0 : set_mode(mode_fbwa, ModeReason::RADIO_FAILSAFE);
86 0 : } else if (g.fs_action_short == FS_ACTION_SHORT_FBWB) {
87 0 : set_mode(mode_fbwb, ModeReason::RADIO_FAILSAFE);
88 : } else {
89 0 : set_mode(mode_circle, ModeReason::RADIO_FAILSAFE);
90 : }
91 : }
92 0 : break;
93 0 : case Mode::Number::CIRCLE: // these modes never take any short failsafe action and continue
94 : case Mode::Number::TAKEOFF:
95 : case Mode::Number::RTL:
96 : #if HAL_QUADPLANE_ENABLED
97 : case Mode::Number::QLAND:
98 : case Mode::Number::QRTL:
99 : case Mode::Number::LOITER_ALT_QLAND:
100 : #endif
101 : case Mode::Number::INITIALISING:
102 0 : break;
103 : }
104 5 : if (failsafe.saved_mode_number != control_mode->mode_number()) {
105 5 : gcs().send_text(MAV_SEVERITY_WARNING, "RC Short Failsafe: switched to %s", control_mode->name());
106 : } else {
107 0 : gcs().send_text(MAV_SEVERITY_WARNING, "RC Short Failsafe On");
108 : }
109 5 : }
110 :
111 10 : void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason)
112 : {
113 :
114 10 : if (reason == ModeReason::GCS_FAILSAFE) {
115 5 : AP_Notify::flags.failsafe_gcs = true;
116 : }
117 :
118 : // This is how to handle a long loss of control signal failsafe.
119 : // If the GCS is locked up we allow control to revert to RC
120 10 : RC_Channels::clear_overrides();
121 10 : failsafe.state = fstype;
122 10 : switch (control_mode->mode_number())
123 : {
124 7 : case Mode::Number::MANUAL:
125 : case Mode::Number::STABILIZE:
126 : case Mode::Number::ACRO:
127 : case Mode::Number::FLY_BY_WIRE_A:
128 : case Mode::Number::AUTOTUNE:
129 : case Mode::Number::FLY_BY_WIRE_B:
130 : case Mode::Number::CRUISE:
131 : case Mode::Number::TRAINING:
132 : case Mode::Number::CIRCLE:
133 : case Mode::Number::LOITER:
134 : case Mode::Number::THERMAL:
135 : case Mode::Number::TAKEOFF:
136 7 : if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF && !(g.fs_action_long == FS_ACTION_LONG_GLIDE || g.fs_action_long == FS_ACTION_LONG_PARACHUTE)) {
137 : // don't failsafe if in initial climb of TAKEOFF mode and FS action is not parachute or glide
138 : // long failsafe will be re-called if still in fs after initial climb
139 0 : long_failsafe_pending = true;
140 0 : break;
141 : }
142 :
143 7 : if(plane.emergency_landing) {
144 0 : set_mode(mode_fbwa, reason); // emergency landing switch overrides normal action to allow out of range landing
145 0 : break;
146 : }
147 7 : if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
148 : #if HAL_PARACHUTE_ENABLED
149 0 : parachute_release();
150 : #endif
151 7 : } else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) {
152 0 : set_mode(mode_fbwa, reason);
153 7 : } else if (g.fs_action_long == FS_ACTION_LONG_AUTO) {
154 0 : set_mode(mode_auto, reason);
155 : #if MODE_AUTOLAND_ENABLED
156 7 : } else if (g.fs_action_long == FS_ACTION_LONG_AUTOLAND) {
157 0 : if (!set_mode(mode_autoland, reason)) {
158 0 : set_mode(mode_rtl, reason);
159 : }
160 : #endif
161 : } else {
162 7 : set_mode(mode_rtl, reason);
163 : }
164 7 : break;
165 :
166 : #if HAL_QUADPLANE_ENABLED
167 0 : case Mode::Number::QSTABILIZE:
168 : case Mode::Number::QHOVER:
169 : case Mode::Number::QLOITER:
170 : case Mode::Number::QACRO:
171 : #if QAUTOTUNE_ENABLED
172 : case Mode::Number::QAUTOTUNE:
173 : #endif
174 0 : if (quadplane.option_is_set(QuadPlane::Option::FS_RTL)) {
175 0 : set_mode(mode_rtl, reason);
176 0 : } else if (quadplane.option_is_set(QuadPlane::Option::FS_QRTL)) {
177 0 : set_mode(mode_qrtl, reason);
178 : } else {
179 0 : set_mode(mode_qland, reason);
180 : }
181 0 : break;
182 : #endif // HAL_QUADPLANE_ENABLED
183 :
184 3 : case Mode::Number::AUTO:
185 3 : if (failsafe_in_landing_sequence()) {
186 : // don't failsafe in a landing sequence
187 0 : break;
188 : }
189 :
190 : #if HAL_QUADPLANE_ENABLED
191 3 : if (quadplane.in_vtol_takeoff()) {
192 0 : set_mode(mode_qland, reason);
193 : // QLAND if in VTOL takeoff
194 0 : break;
195 : }
196 : #endif
197 : FALLTHROUGH;
198 :
199 : case Mode::Number::AVOID_ADSB:
200 : case Mode::Number::GUIDED:
201 :
202 3 : if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
203 : #if HAL_PARACHUTE_ENABLED
204 1 : parachute_release();
205 : #endif
206 2 : } else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) {
207 1 : set_mode(mode_fbwa, reason);
208 1 : } else if (g.fs_action_long == FS_ACTION_LONG_AUTO) {
209 0 : set_mode(mode_auto, reason);
210 : #if MODE_AUTOLAND_ENABLED
211 1 : } else if (g.fs_action_long == FS_ACTION_LONG_AUTOLAND) {
212 0 : if (!set_mode(mode_autoland, reason)) {
213 0 : set_mode(mode_rtl, reason);
214 : }
215 : #endif
216 1 : } else if (g.fs_action_long == FS_ACTION_LONG_RTL) {
217 0 : set_mode(mode_rtl, reason);
218 : }
219 3 : break;
220 0 : case Mode::Number::RTL:
221 0 : if (g.fs_action_long == FS_ACTION_LONG_AUTO) {
222 0 : set_mode(mode_auto, reason);
223 : #if MODE_AUTOLAND_ENABLED
224 0 : } else if (g.fs_action_long == FS_ACTION_LONG_AUTOLAND) {
225 0 : set_mode(mode_autoland, reason);
226 : #endif
227 : }
228 0 : break;
229 : #if HAL_QUADPLANE_ENABLED
230 0 : case Mode::Number::QLAND:
231 : case Mode::Number::QRTL:
232 : case Mode::Number::LOITER_ALT_QLAND:
233 : #endif
234 : case Mode::Number::INITIALISING:
235 : #if MODE_AUTOLAND_ENABLED
236 : case Mode::Number::AUTOLAND:
237 : #endif
238 0 : break;
239 : }
240 10 : gcs().send_text(MAV_SEVERITY_WARNING, "%s Failsafe On: switched to %s", (reason == ModeReason:: GCS_FAILSAFE) ? "GCS" : "RC Long", control_mode->name());
241 10 : }
242 :
243 2 : void Plane::rc_failsafe_short_off_event()
244 : {
245 : // We're back in radio contact
246 2 : gcs().send_text(MAV_SEVERITY_WARNING, "RC Short Failsafe Cleared");
247 2 : failsafe.state = FAILSAFE_NONE;
248 : // restore entry mode if desired but check that our current mode is still due to failsafe
249 2 : if (control_mode_reason == ModeReason::RADIO_FAILSAFE) {
250 2 : set_mode_by_number(failsafe.saved_mode_number, ModeReason::RADIO_FAILSAFE_RECOVERY);
251 2 : gcs().send_text(MAV_SEVERITY_INFO,"Flight mode %s restored",control_mode->name());
252 : }
253 2 : }
254 :
255 10 : void Plane::failsafe_long_off_event(ModeReason reason)
256 : {
257 10 : long_failsafe_pending = false;
258 : // We're back in radio contact with RC or GCS
259 10 : if (reason == ModeReason:: GCS_FAILSAFE) {
260 5 : AP_Notify::flags.failsafe_gcs = false;
261 5 : gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe Cleared");
262 : }
263 : else {
264 5 : gcs().send_text(MAV_SEVERITY_WARNING, "RC Long Failsafe Cleared");
265 : }
266 10 : failsafe.state = FAILSAFE_NONE;
267 10 : }
268 :
269 2 : void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
270 : {
271 2 : switch ((Failsafe_Action)action) {
272 : #if HAL_QUADPLANE_ENABLED
273 2 : case Failsafe_Action_Loiter_alt_QLand:
274 2 : if (quadplane.available()) {
275 2 : plane.set_mode(mode_loiter_qland, ModeReason::BATTERY_FAILSAFE);
276 2 : break;
277 : }
278 : FALLTHROUGH;
279 :
280 : case Failsafe_Action_QLand:
281 0 : if (quadplane.available()) {
282 0 : plane.set_mode(mode_qland, ModeReason::BATTERY_FAILSAFE);
283 0 : break;
284 : }
285 : FALLTHROUGH;
286 : #endif // HAL_QUADPLANE_ENABLED
287 : case Failsafe_Action_Land: {
288 0 : bool already_landing = flight_stage == AP_FixedWing::FlightStage::LAND;
289 : #if HAL_QUADPLANE_ENABLED
290 0 : if (control_mode == &mode_qland || control_mode == &mode_loiter_qland) {
291 0 : already_landing = true;
292 : }
293 : #endif
294 0 : if (!already_landing && plane.have_position) {
295 : // never stop a landing if we were already committed
296 0 : if (plane.mission.is_best_land_sequence(plane.current_loc)) {
297 : // continue mission as it will reach a landing in less distance
298 0 : plane.mission.set_in_landing_sequence_flag(true);
299 0 : break;
300 : }
301 0 : if (plane.mission.jump_to_landing_sequence(plane.current_loc)) {
302 0 : plane.set_mode(mode_auto, ModeReason::BATTERY_FAILSAFE);
303 0 : break;
304 : }
305 : }
306 : FALLTHROUGH;
307 : }
308 : case Failsafe_Action_RTL:
309 : case Failsafe_Action_AUTOLAND_OR_RTL: {
310 0 : bool already_landing = flight_stage == AP_FixedWing::FlightStage::LAND;
311 : #if HAL_QUADPLANE_ENABLED
312 0 : if (control_mode == &mode_qland || control_mode == &mode_loiter_qland ||
313 0 : quadplane.in_vtol_land_sequence()) {
314 0 : already_landing = true;
315 : }
316 : #endif
317 : #if MODE_AUTOLAND_ENABLED
318 0 : if (control_mode == &mode_autoland) {
319 0 : already_landing = true;
320 : }
321 : #endif
322 0 : if (!already_landing) {
323 : // never stop a landing if we were already committed
324 0 : if ((g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START) && plane.have_position && plane.mission.is_best_land_sequence(plane.current_loc)) {
325 : // continue mission as it will reach a landing in less distance
326 0 : plane.mission.set_in_landing_sequence_flag(true);
327 0 : break;
328 : }
329 0 : aparm.throttle_cruise.load();
330 : #if MODE_AUTOLAND_ENABLED
331 0 : if (((Failsafe_Action)action == Failsafe_Action_AUTOLAND_OR_RTL) && set_mode(mode_autoland, ModeReason::BATTERY_FAILSAFE)) {
332 0 : break;
333 : }
334 : #endif
335 0 : set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE);
336 : }
337 0 : break;
338 : }
339 :
340 0 : case Failsafe_Action_Terminate:
341 : #if AP_ADVANCEDFAILSAFE_ENABLED
342 : char battery_type_str[17];
343 0 : snprintf(battery_type_str, 17, "%s battery", type_str);
344 0 : afs.gcs_terminate(true, battery_type_str);
345 : #else
346 : arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE);
347 : #endif
348 0 : break;
349 :
350 0 : case Failsafe_Action_Parachute:
351 : #if HAL_PARACHUTE_ENABLED
352 0 : parachute_release();
353 : #endif
354 0 : break;
355 :
356 0 : case Failsafe_Action_None:
357 : // don't actually do anything, however we should still flag the system as having hit a failsafe
358 : // and ensure all appropriate flags are going off to the user
359 0 : break;
360 : }
361 2 : }
|
