Line data Source code
1 : /*
2 : additional arming checks for plane
3 : */
4 : #include "AP_Arming_Plane.h"
5 : #include "Plane.h"
6 :
7 : #include "qautotune.h"
8 :
9 : constexpr uint32_t AP_ARMING_DELAY_MS = 2000; // delay from arming to start of motor spoolup
10 :
11 : const AP_Param::GroupInfo AP_Arming_Plane::var_info[] = {
12 : // variables from parent vehicle
13 : AP_NESTEDGROUPINFO(AP_Arming, 0),
14 :
15 : // index 3 was RUDDER and should not be used
16 :
17 : #if AP_PLANE_BLACKBOX_LOGGING
18 : // @Param: BBOX_SPD
19 : // @DisplayName: Blackbox speed
20 : // @Description: This is a 3D GPS speed threshold above which we will force arm the vehicle to start logging. WARNING: This should only be used on a vehicle with no propellers attached to the flight controller and when the flight controller is not in control of the vehicle.
21 : // @Units: m/s
22 : // @Increment: 1
23 : // @Range: 1 20
24 : // @User: Advanced
25 : AP_GROUPINFO("BBOX_SPD", 4, AP_Arming_Plane, blackbox_speed, 5),
26 : #endif // AP_PLANE_BLACKBOX_LOGGING
27 :
28 : AP_GROUPEND
29 : };
30 :
31 : // expected to return true if the terrain database is required to have
32 : // all data loaded
33 26163 : bool AP_Arming_Plane::terrain_database_required() const
34 : {
35 : #if AP_TERRAIN_AVAILABLE
36 26163 : if (plane.g.terrain_follow) {
37 408 : return true;
38 : }
39 : #endif
40 25755 : return AP_Arming::terrain_database_required();
41 : }
42 :
43 : /*
44 : additional arming checks for plane
45 :
46 : */
47 109082 : bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
48 : {
49 109082 : if (armed || require == (uint8_t)Required::NO) {
50 : // if we are already armed or don't need any arming checks
51 : // then skip the checks
52 82841 : return true;
53 : }
54 26241 : if (!hal.scheduler->is_system_initialized()) {
55 0 : check_failed(display_failure, "System not initialised");
56 0 : return false;
57 : }
58 : // are arming checks disabled?
59 26241 : if (should_skip_all_checks()) {
60 78 : return mandatory_checks(display_failure);
61 : }
62 26163 : if (hal.util->was_watchdog_armed()) {
63 : // on watchdog reset bypass arming checks to allow for
64 : // in-flight arming if we were armed before the reset. This
65 : // allows a reset on a BVLOS flight to return home if the
66 : // operator can command arming over telemetry
67 0 : return true;
68 : }
69 :
70 : // call parent class checks
71 26163 : bool ret = AP_Arming::pre_arm_checks(display_failure);
72 :
73 : #if AP_AIRSPEED_ENABLED
74 : // Check airspeed sensor
75 26163 : ret &= AP_Arming::airspeed_checks(display_failure);
76 : #endif
77 :
78 26163 : if (plane.aparm.roll_limit < 3) {
79 0 : check_failed(display_failure, "ROLL_LIMIT_DEG too small (%.1f)", plane.aparm.roll_limit.get());
80 0 : ret = false;
81 : }
82 :
83 26163 : if (plane.aparm.pitch_limit_max < 3) {
84 0 : check_failed(display_failure, "PTCH_LIM_MAX_DEG too small (%.1f)", plane.aparm.pitch_limit_max.get());
85 0 : ret = false;
86 : }
87 :
88 26163 : if (plane.aparm.pitch_limit_min > -3) {
89 0 : check_failed(display_failure, "PTCH_LIM_MIN_DEG too large (%.1f)", plane.aparm.pitch_limit_min.get());
90 0 : ret = false;
91 : }
92 :
93 26163 : if (plane.aparm.airspeed_min < MIN_AIRSPEED_MIN) {
94 0 : check_failed(display_failure, "AIRSPEED_MIN too low (%i < %i)", plane.aparm.airspeed_min.get(), MIN_AIRSPEED_MIN);
95 0 : ret = false;
96 : }
97 :
98 26163 : if (plane.channel_throttle->get_reverse() &&
99 26163 : plane.g.throttle_fs_enabled != Plane::ThrFailsafe::Disabled &&
100 0 : plane.g.throttle_fs_value <
101 0 : plane.channel_throttle->get_radio_max()) {
102 0 : check_failed(display_failure, "Invalid THR_FS_VALUE for rev throttle");
103 0 : ret = false;
104 : }
105 :
106 26163 : ret &= rc_received_if_enabled_check(display_failure);
107 :
108 : #if HAL_QUADPLANE_ENABLED
109 26163 : ret &= quadplane_checks(display_failure);
110 : #endif
111 :
112 : // check adsb avoidance failsafe
113 26163 : if (plane.failsafe.adsb) {
114 8 : check_failed(display_failure, "ADSB threat detected");
115 8 : ret = false;
116 : }
117 :
118 26163 : if (plane.flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM)){
119 0 : int16_t trim = plane.channel_throttle->get_radio_trim();
120 0 : if (trim < 1250 || trim > 1750) {
121 0 : check_failed(display_failure, "Throttle trim not near center stick(%u)",trim );
122 0 : ret = false;
123 : }
124 : }
125 :
126 26848 : if (plane.mission.get_in_landing_sequence_flag() &&
127 685 : !plane.mission.starts_with_takeoff_cmd()) {
128 685 : check_failed(display_failure,"In landing sequence");
129 685 : ret = false;
130 : }
131 :
132 26163 : char failure_msg[50] {};
133 26163 : if (!plane.control_mode->pre_arm_checks(ARRAY_SIZE(failure_msg), failure_msg)) {
134 466 : check_failed(display_failure, "%s %s", plane.control_mode->name(), failure_msg);
135 466 : return false;
136 : }
137 :
138 25697 : return ret;
139 : }
140 :
141 164 : bool AP_Arming_Plane::mandatory_checks(bool display_failure)
142 : {
143 164 : bool ret = true;
144 :
145 164 : ret &= rc_received_if_enabled_check(display_failure);
146 :
147 : // Call parent class checks
148 164 : ret &= AP_Arming::mandatory_checks(display_failure);
149 :
150 164 : return ret;
151 : }
152 :
153 :
154 : #if HAL_QUADPLANE_ENABLED
155 26163 : bool AP_Arming_Plane::quadplane_checks(bool display_failure)
156 : {
157 26163 : if (!plane.quadplane.enabled()) {
158 16267 : return true;
159 : }
160 :
161 9896 : if (!plane.quadplane.available()) {
162 0 : check_failed(display_failure, "Quadplane enabled but not running");
163 0 : return false;
164 : }
165 :
166 9896 : bool ret = true;
167 :
168 9896 : if (plane.scheduler.get_loop_rate_hz() < 100) {
169 0 : check_failed(display_failure, "quadplane needs SCHED_LOOP_RATE >= 100");
170 0 : ret = false;
171 : }
172 :
173 9896 : char failure_msg[50] {};
174 9896 : if (!plane.quadplane.motors->arming_checks(ARRAY_SIZE(failure_msg), failure_msg)) {
175 0 : check_failed(display_failure, "Motors: %s", failure_msg);
176 0 : ret = false;
177 : }
178 :
179 9896 : if ((plane.quadplane.tailsitter.enable > 0) && (plane.quadplane.tiltrotor.enable > 0)) {
180 0 : check_failed(Check::PARAMETERS, display_failure, "set TAILSIT_ENABLE 0 or TILT_ENABLE 0");
181 0 : ret = false;
182 :
183 : } else {
184 :
185 9896 : if ((plane.quadplane.tailsitter.enable > 0) && !plane.quadplane.tailsitter.enabled()) {
186 2 : check_failed(Check::PARAMETERS, display_failure, "tailsitter setup not complete, reboot");
187 2 : ret = false;
188 : }
189 :
190 9896 : if ((plane.quadplane.tiltrotor.enable > 0) && !plane.quadplane.tiltrotor.enabled()) {
191 0 : check_failed(Check::PARAMETERS, display_failure, "tiltrotor setup not complete, reboot");
192 0 : ret = false;
193 : }
194 : }
195 :
196 : // ensure controllers are OK with us arming:
197 9896 : if (!plane.quadplane.pos_control->pre_arm_checks("Q_P", failure_msg, ARRAY_SIZE(failure_msg))) {
198 3 : check_failed(Check::PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
199 3 : ret = false;
200 : }
201 9896 : if (!plane.quadplane.attitude_control->pre_arm_checks("Q_A", failure_msg, ARRAY_SIZE(failure_msg))) {
202 0 : check_failed(Check::PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
203 0 : ret = false;
204 : }
205 :
206 : /*
207 : Q_ASSIST_SPEED really should be enabled for all quadplanes except tailsitters
208 : */
209 19792 : if (check_enabled(Check::PARAMETERS) &&
210 19792 : is_zero(plane.quadplane.assist.speed) &&
211 316 : !plane.quadplane.tailsitter.enabled()) {
212 316 : check_failed(display_failure,"Q_ASSIST_SPEED is not set");
213 316 : ret = false;
214 : }
215 :
216 9896 : if ((plane.quadplane.tailsitter.enable > 0) && (plane.quadplane.q_fwd_thr_use != QuadPlane::FwdThrUse::OFF)) {
217 0 : check_failed(Check::PARAMETERS, display_failure, "set Q_FWD_THR_USE to 0");
218 0 : ret = false;
219 : }
220 :
221 : // combining Q_RTL_MODE with either of the RTL_AUTOLAND options
222 : // leads to precedence questions, so just don't allow it:
223 10149 : if (plane.g.rtl_autoland != RtlAutoland::RTL_DISABLE &&
224 253 : plane.quadplane.rtl_mode != QuadPlane::RTL_MODE::NONE) {
225 1 : check_failed(Check::PARAMETERS, display_failure, "unset one of RTL_AUTOLAND or Q_RTL_MODE");
226 1 : ret = false;
227 : }
228 :
229 9896 : return ret;
230 : }
231 : #endif // HAL_QUADPLANE_ENABLED
232 :
233 26163 : bool AP_Arming_Plane::ins_checks(bool display_failure)
234 : {
235 : // call parent class checks
236 26163 : if (!AP_Arming::ins_checks(display_failure)) {
237 9026 : return false;
238 : }
239 :
240 : // additional plane specific checks
241 17137 : if (check_enabled(Check::INS)) {
242 17137 : char failure_msg[50] = {};
243 17137 : if (!AP::ahrs().pre_arm_check(true, failure_msg, sizeof(failure_msg))) {
244 5818 : check_failed(Check::INS, display_failure, "AHRS: %s", failure_msg);
245 5818 : return false;
246 : }
247 : }
248 :
249 11319 : return true;
250 : }
251 :
252 329 : bool AP_Arming_Plane::arm_checks(AP_Arming::Method method)
253 : {
254 :
255 : // are arming checks disabled?
256 329 : if (should_skip_all_checks()) {
257 0 : return true;
258 : }
259 :
260 329 : if (hal.util->was_watchdog_armed()) {
261 : // on watchdog reset bypass arming checks to allow for
262 : // in-flight arming if we were armed before the reset. This
263 : // allows a reset on a BVLOS flight to return home if the
264 : // operator can command arming over telemetry
265 0 : gcs().send_text(MAV_SEVERITY_WARNING, "watchdog: Bypassing arming checks");
266 0 : return true;
267 : }
268 :
269 : // call parent class checks
270 329 : return AP_Arming::arm_checks(method);
271 : }
272 :
273 : /*
274 : update HAL soft arm state
275 : */
276 808 : void AP_Arming_Plane::change_arm_state(void)
277 : {
278 808 : update_soft_armed();
279 : #if HAL_QUADPLANE_ENABLED
280 808 : plane.quadplane.set_armed(hal.util->get_soft_armed());
281 : #endif
282 808 : }
283 :
284 430 : bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_checks)
285 : {
286 430 : if (!AP_Arming::arm(method, do_arming_checks)) {
287 15 : return false;
288 : }
289 :
290 415 : if (plane.update_home()) {
291 : // after update_home the home position could still be
292 : // different from the current_loc if the EKF refused the
293 : // resetHeightDatum call. If we are updating home then we want
294 : // to force the home to be the current_loc so relative alt
295 : // takeoffs work correctly
296 319 : if (plane.ahrs.set_home(plane.current_loc)) {
297 : // update current_loc
298 319 : plane.update_current_loc();
299 : }
300 : }
301 :
302 415 : change_arm_state();
303 :
304 : // rising edge of delay_arming oneshot
305 415 : delay_arming = true;
306 :
307 : #if MODE_AUTOLAND_ENABLED
308 415 : plane.mode_autoland.arm_check();
309 : #endif
310 :
311 415 : if (method == AP_Arming::Method::RUDDER) {
312 : // initialise the timer used to warn the user they're holding
313 : // their stick over:
314 5 : plane.takeoff_state.rudder_takeoff_warn_ms = AP_HAL::millis();
315 : }
316 :
317 415 : send_arm_disarm_statustext("Throttle armed");
318 :
319 415 : return true;
320 : }
321 :
322 : /*
323 : disarm motors
324 : */
325 717 : bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_checks)
326 : {
327 1832 : if (do_disarm_checks &&
328 1115 : (AP_Arming::method_is_GCS(method) ||
329 : method == AP_Arming::Method::RUDDER)) {
330 83 : if (plane.is_flying()) {
331 : // don't allow mavlink or rudder disarm while flying
332 21 : return false;
333 : }
334 : }
335 :
336 696 : if (!AP_Arming::disarm(method, do_disarm_checks)) {
337 303 : return false;
338 : }
339 393 : if (plane.control_mode != &plane.mode_auto) {
340 : // reset the mission on disarm if we are not in auto
341 244 : plane.mission.reset();
342 : }
343 :
344 : // suppress the throttle in auto-throttle modes
345 393 : plane.throttle_suppressed = plane.control_mode->does_auto_throttle();
346 :
347 : // if no airmode switch assigned, ensure airmode is off:
348 : #if HAL_QUADPLANE_ENABLED
349 393 : if ((plane.quadplane.air_mode == AirMode::ON) && (rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRMODE) == nullptr)) {
350 17 : plane.quadplane.air_mode = AirMode::OFF;
351 : }
352 : #endif
353 :
354 : //only log if disarming was successful
355 393 : change_arm_state();
356 :
357 : #if QAUTOTUNE_ENABLED
358 : // Possibly save auto tuned parameters
359 393 : plane.quadplane.qautotune.disarmed(plane.control_mode == &plane.mode_qautotune);
360 : #endif
361 :
362 : // re-initialize speed variable used in AUTO and GUIDED for
363 : // DO_CHANGE_SPEED commands
364 393 : plane.new_airspeed_cm = -1;
365 :
366 : #if MODE_AUTOLAND_ENABLED
367 : // takeoff direction always cleared on disarm
368 393 : plane.takeoff_state.initial_direction.initialized = false;
369 : #endif
370 393 : send_arm_disarm_statustext("Throttle disarmed");
371 393 : return true;
372 : }
373 :
374 8378355 : void AP_Arming_Plane::update_soft_armed()
375 : {
376 8378355 : bool _armed = is_armed();
377 : #if HAL_QUADPLANE_ENABLED
378 8378355 : if (plane.quadplane.motor_test.running){
379 6000 : _armed = true;
380 : }
381 : #endif
382 :
383 8378355 : hal.util->set_soft_armed(_armed);
384 : #if HAL_LOGGING_ENABLED
385 8378355 : AP::logger().set_vehicle_armed(hal.util->get_soft_armed());
386 : #endif
387 :
388 : // update delay_arming oneshot
389 8558540 : if (delay_arming &&
390 180185 : (AP_HAL::millis() - hal.util->get_last_armed_change() >= AP_ARMING_DELAY_MS)) {
391 :
392 396 : delay_arming = false;
393 : }
394 :
395 : #if AP_PLANE_BLACKBOX_LOGGING
396 : if (blackbox_speed > 0) {
397 : const float speed3d = plane.gps.status() >= AP_GPS_FixType::FIX_3D?plane.gps.velocity().length():0;
398 : const uint32_t now = AP_HAL::millis();
399 : if (speed3d > blackbox_speed) {
400 : last_over_3dspeed_ms = now;
401 : }
402 : if (!_armed && speed3d > blackbox_speed) {
403 : // force safety on so we don't run motors
404 : hal.rcout->force_safety_on();
405 : AP_Param::set_by_name("RC_PROTOCOLS", 0);
406 : arm(Method::BLACKBOX, false);
407 : gcs().send_text(MAV_SEVERITY_WARNING, "BlackBox: arming at %.1f m/s", speed3d);
408 : }
409 : if (_armed && now - last_over_3dspeed_ms > 20000U) {
410 : gcs().send_text(MAV_SEVERITY_WARNING, "BlackBox: disarming at %.1f m/s", speed3d);
411 : disarm(Method::BLACKBOX, false);
412 : }
413 : }
414 : #endif
415 8378355 : }
416 :
417 : #if AP_MISSION_ENABLED
418 : /*
419 : extra plane mission checks
420 : */
421 26163 : bool AP_Arming_Plane::mission_checks(bool report)
422 : {
423 : // base checks
424 26163 : bool ret = AP_Arming::mission_checks(report);
425 26163 : if (plane.g.rtl_autoland == RtlAutoland::RTL_DISABLE) {
426 25136 : if (plane.mission.contains_item(MAV_CMD_DO_LAND_START)) {
427 736 : ret = false;
428 736 : check_failed(Check::MISSION, report, "DO_LAND_START set and RTL_AUTOLAND disabled");
429 : }
430 25136 : if (plane.mission.contains_item(MAV_CMD_DO_RETURN_PATH_START)) {
431 0 : ret = false;
432 0 : check_failed(Check::MISSION, report, "DO_RETURN_PATH_START set and RTL_AUTOLAND disabled");
433 : }
434 : }
435 : #if HAL_QUADPLANE_ENABLED
436 26163 : if (plane.quadplane.available()) {
437 9896 : const uint16_t num_commands = plane.mission.num_commands();
438 9896 : AP_Mission::Mission_Command prev_cmd {};
439 16315 : for (uint16_t i=1; i<num_commands; i++) {
440 6419 : AP_Mission::Mission_Command cmd;
441 6419 : if (!plane.mission.read_cmd_from_storage(i, cmd)) {
442 0 : break;
443 : }
444 6982 : if (plane.is_land_command(cmd.id) &&
445 563 : prev_cmd.id == MAV_CMD_NAV_WAYPOINT) {
446 417 : const float dist = cmd.content.location.get_distance(prev_cmd.content.location);
447 417 : const float tecs_land_speed = plane.TECS_controller.get_land_airspeed();
448 417 : const float landing_speed = is_positive(tecs_land_speed)?tecs_land_speed:plane.aparm.airspeed_cruise;
449 417 : const float min_dist = 0.75 * plane.quadplane.stopping_distance_m(sq(landing_speed));
450 417 : if (dist < min_dist) {
451 0 : ret = false;
452 0 : check_failed(Check::MISSION, report, "VTOL land too short, min %.0fm", min_dist);
453 : }
454 : }
455 6419 : prev_cmd = cmd;
456 : }
457 : }
458 : #endif // HAL_QUADPLANE_ENABLED
459 26163 : return ret;
460 : }
461 : #endif // AP_MISSION_ENABLED
462 :
463 : // Checks rc has been received if it is configured to be used
464 26327 : bool AP_Arming_Plane::rc_received_if_enabled_check(bool display_failure)
465 : {
466 26327 : if (rc().enabled_protocols() == 0) {
467 : // No protocols enabled, will never get RC, don't block arming
468 0 : return true;
469 : }
470 :
471 : // If RC failsafe is enabled we must receive RC before arming
472 52553 : if ((plane.g.throttle_fs_enabled == Plane::ThrFailsafe::Enabled) &&
473 26226 : !(rc().has_had_rc_receiver() || rc().has_had_rc_override())) {
474 8 : check_failed(display_failure, "Waiting for RC");
475 8 : return false;
476 : }
477 :
478 26319 : return true;
479 : }
|
