Line data Source code
1 : #include "Copter.h"
2 :
3 : #pragma GCC diagnostic push
4 : #if defined(__clang_major__) && __clang_major__ >= 14
5 : #pragma GCC diagnostic ignored "-Wbitwise-instead-of-logical"
6 : #endif
7 :
8 142354 : bool AP_Arming_Copter::pre_arm_checks(bool display_failure)
9 : {
10 142354 : const bool passed = run_pre_arm_checks(display_failure);
11 142354 : set_pre_arm_check(passed);
12 142354 : return passed;
13 : }
14 :
15 : // perform pre-arm checks
16 : // return true if the checks pass successfully
17 142354 : bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure)
18 : {
19 : // exit immediately if already armed
20 142354 : if (copter.motors->armed()) {
21 49313 : return true;
22 : }
23 :
24 93041 : if (!hal.scheduler->is_system_initialized()) {
25 0 : check_failed(display_failure, "System not initialised");
26 0 : return false;
27 : }
28 :
29 : // check if motor interlock and either Emergency Stop aux switches are used
30 : // at the same time. This cannot be allowed.
31 93041 : bool passed = true;
32 98411 : if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK) &&
33 5370 : (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP) ||
34 2685 : rc().find_channel_for_option(RC_Channel::AUX_FUNC::ARM_EMERGENCY_STOP))){
35 0 : check_failed(display_failure, "Interlock/E-Stop Conflict");
36 0 : passed = false;
37 : }
38 :
39 : // check if motor interlock aux switch is in use
40 : // if it is, switch needs to be in disabled position to arm
41 : // otherwise exit immediately.
42 93041 : if (copter.ap.using_interlock && copter.ap.motor_interlock_switch) {
43 155 : check_failed(display_failure, "Motor Interlock Enabled");
44 155 : passed = false;
45 : }
46 :
47 93041 : if (!disarm_switch_checks(display_failure)) {
48 0 : passed = false;
49 : }
50 :
51 : // always check motors
52 93041 : char failure_msg[100] {};
53 93041 : if (!copter.motors->arming_checks(ARRAY_SIZE(failure_msg), failure_msg)) {
54 61 : check_failed(display_failure, "Motors: %s", failure_msg);
55 61 : passed = false;
56 : }
57 :
58 : #if FRAME_CONFIG == HELI_FRAME && MODE_AUTOROTATE_ENABLED
59 : // check on autorotation config
60 2339 : if (!copter.g2.arot.arming_checks(ARRAY_SIZE(failure_msg), failure_msg)) {
61 91 : check_failed(display_failure, "AROT: %s", failure_msg);
62 91 : passed = false;
63 : }
64 : #endif
65 :
66 : // If not passed all checks return false
67 93041 : if (!passed) {
68 307 : return false;
69 : }
70 :
71 : // if pre arm checks are disabled run only the mandatory checks
72 92734 : if (should_skip_all_checks()) {
73 18 : return mandatory_checks(display_failure);
74 : }
75 :
76 : // bitwise & ensures all checks are run
77 92716 : return parameter_checks(display_failure)
78 92716 : & oa_checks(display_failure)
79 92716 : & gcs_failsafe_check(display_failure)
80 92716 : & winch_checks(display_failure)
81 92716 : & rc_throttle_failsafe_checks(display_failure)
82 92716 : & alt_checks(display_failure)
83 : #if AP_AIRSPEED_ENABLED
84 92716 : & AP_Arming::airspeed_checks(display_failure)
85 : #endif
86 92716 : & AP_Arming::pre_arm_checks(display_failure);
87 : }
88 :
89 92716 : bool AP_Arming_Copter::rc_throttle_failsafe_checks(bool display_failure) const
90 : {
91 92716 : if (!check_enabled(Check::RC)) {
92 : // this check has been disabled
93 2727 : return true;
94 : }
95 :
96 : // throttle failsafe. In this case the parameter also gates the
97 : // no-pulses RC failure case - the radio-in value can be zero due
98 : // to not having received any RC pulses at all. Note that if we
99 : // have ever seen RC and then we *lose* RC then these checks are
100 : // likely to pass if the user is relying on no-pulses to detect RC
101 : // failure. However, arming is precluded in that case by being in
102 : // RC failsafe.
103 89989 : if (copter.g.failsafe_throttle == Copter::FS_THR_Action::DISABLED) {
104 111 : return true;
105 : }
106 :
107 : #if FRAME_CONFIG == HELI_FRAME
108 2149 : const char *rc_item = "Collective";
109 : #else
110 87729 : const char *rc_item = "Throttle";
111 : #endif
112 :
113 89878 : if (!rc().has_had_rc_receiver() && !rc().has_had_rc_override()) {
114 12 : check_failed(Check::RC, display_failure, "RC not found");
115 12 : return false;
116 : }
117 :
118 : // check throttle is not too low - must be above failsafe throttle
119 89866 : if (copter.channel_throttle->get_radio_in() < copter.g.failsafe_throttle_value) {
120 33 : check_failed(Check::RC, display_failure, "%s below failsafe", rc_item);
121 33 : return false;
122 : }
123 :
124 89833 : return true;
125 : }
126 :
127 92716 : bool AP_Arming_Copter::barometer_checks(bool display_failure)
128 : {
129 92716 : if (!AP_Arming::barometer_checks(display_failure)) {
130 10 : return false;
131 : }
132 :
133 92706 : bool ret = true;
134 : // check Baro
135 92706 : if (check_enabled(Check::BARO)) {
136 : // Check baro & inav alt are within 1m if EKF is operating in an absolute position mode.
137 : // Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height
138 : // that may differ from the baro height due to baro drift.
139 89979 : const auto &ahrs = AP::ahrs();
140 89979 : const bool using_baro_ref = !ahrs.has_status(AP_AHRS::Status::PRED_HORIZ_POS_REL) && ahrs.has_status(AP_AHRS::Status::PRED_HORIZ_POS_ABS);
141 89979 : float pos_d_m = 0;
142 89979 : UNUSED_RESULT(AP::ahrs().get_relative_position_D_origin_float(pos_d_m));
143 89979 : if (using_baro_ref) {
144 0 : if (fabsf(-pos_d_m - copter.baro_alt_m) > PREARM_MAX_ALT_DISPARITY_M) {
145 0 : check_failed(Check::BARO, display_failure, "Altitude disparity");
146 0 : ret = false;
147 : }
148 : }
149 : }
150 92706 : return ret;
151 : }
152 :
153 92716 : bool AP_Arming_Copter::ins_checks(bool display_failure)
154 : {
155 92716 : bool ret = AP_Arming::ins_checks(display_failure);
156 :
157 92716 : if (check_enabled(Check::INS)) {
158 :
159 : // get ekf attitude (if bad, it's usually the gyro biases)
160 89989 : if (!pre_arm_ekf_attitude_check()) {
161 69004 : check_failed(Check::INS, display_failure, "EKF attitude is bad");
162 69004 : ret = false;
163 : }
164 : }
165 :
166 92716 : return ret;
167 : }
168 :
169 92716 : bool AP_Arming_Copter::board_voltage_checks(bool display_failure)
170 : {
171 92716 : if (!AP_Arming::board_voltage_checks(display_failure)) {
172 0 : return false;
173 : }
174 :
175 : // check battery voltage
176 92716 : if (check_enabled(Check::VOLTAGE)) {
177 89989 : if (copter.battery.has_failsafed()) {
178 27 : check_failed(Check::VOLTAGE, display_failure, "Battery failsafe");
179 27 : return false;
180 : }
181 : }
182 :
183 92689 : return true;
184 : }
185 :
186 : // expected to return true if the terrain database is required to have
187 : // all data loaded
188 89989 : bool AP_Arming_Copter::terrain_database_required() const
189 : {
190 :
191 89989 : if (copter.wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER) {
192 : // primary terrain source is from rangefinder, allow arming without terrain database
193 1789 : return false;
194 : }
195 :
196 176322 : if (copter.wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_TERRAINDATABASE &&
197 88122 : copter.mode_rtl.get_alt_type() == ModeRTL::RTLAltType::TERRAIN) {
198 3 : return true;
199 : }
200 88197 : return AP_Arming::terrain_database_required();
201 : }
202 :
203 92716 : bool AP_Arming_Copter::parameter_checks(bool display_failure)
204 : {
205 : // check various parameter values
206 92716 : if (check_enabled(Check::PARAMETERS)) {
207 :
208 : // failsafe parameter checks
209 89989 : if (copter.g.failsafe_throttle) {
210 : // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900
211 89878 : if (copter.channel_throttle->get_radio_min() <= copter.g.failsafe_throttle_value+10 || copter.g.failsafe_throttle_value < 910) {
212 0 : check_failed(Check::PARAMETERS, display_failure, "Check FS_THR_VALUE");
213 9 : return false;
214 : }
215 : }
216 89989 : if (copter.g.failsafe_gcs == Copter::FS_GCS_Action::CONTINUE_MISSION) {
217 : // FS_GCS_ENABLE == 2 has been removed
218 0 : check_failed(Check::PARAMETERS, display_failure, "FS_GCS_ENABLE=2 removed, see FS_OPTIONS");
219 : }
220 :
221 : // acro balance parameter check
222 : #if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED
223 179978 : if (is_negative(copter.g.acro_balance_roll) || is_negative(copter.g.acro_balance_pitch) ||
224 269967 : (copter.g.acro_balance_roll > copter.attitude_control->get_angle_roll_p().kP()) ||
225 89989 : (copter.g.acro_balance_pitch > copter.attitude_control->get_angle_pitch_p().kP())) {
226 0 : check_failed(Check::PARAMETERS, display_failure, "Check ACRO_BAL_ROLL/PITCH");
227 0 : return false;
228 : }
229 : #endif
230 :
231 : // pilot-speed-up parameter check
232 89989 : if (copter.g2.pilot_speed_up_ms <= 0) {
233 0 : check_failed(Check::PARAMETERS, display_failure, "Check PILOT_SPD_UP");
234 0 : return false;
235 : }
236 :
237 : #if FRAME_CONFIG == HELI_FRAME
238 2149 : char fail_msg[100]{};
239 : // check input manager parameters
240 2149 : if (!copter.input_manager.parameter_check(fail_msg, ARRAY_SIZE(fail_msg))) {
241 0 : check_failed(Check::PARAMETERS, display_failure, "%s", fail_msg);
242 0 : return false;
243 : }
244 :
245 : // Ensure an Aux Channel is configured for motor interlock
246 2149 : if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK) == nullptr) {
247 0 : check_failed(Check::PARAMETERS, display_failure, "Motor Interlock not configured");
248 0 : return false;
249 : }
250 :
251 : #else
252 87840 : switch (copter.g2.frame_class.get()) {
253 0 : case AP_Motors::MOTOR_FRAME_HELI_QUAD:
254 : case AP_Motors::MOTOR_FRAME_HELI_DUAL:
255 : case AP_Motors::MOTOR_FRAME_HELI:
256 0 : check_failed(Check::PARAMETERS, display_failure, "Invalid MultiCopter FRAME_CLASS");
257 0 : return false;
258 :
259 87840 : default:
260 87840 : break;
261 : }
262 : #endif // HELI_FRAME
263 :
264 : // checks when using range finder for RTL
265 : #if MODE_RTL_ENABLED
266 89989 : if (copter.mode_rtl.get_alt_type() == ModeRTL::RTLAltType::TERRAIN) {
267 : // get terrain source from wpnav
268 449 : const char *failure_template = "RTL_ALT_TYPE is above-terrain but %s";
269 449 : switch (copter.wp_nav->get_terrain_source()) {
270 0 : case AC_WPNav::TerrainSource::TERRAIN_UNAVAILABLE:
271 0 : check_failed(Check::PARAMETERS, display_failure, failure_template, "no terrain data");
272 0 : return false;
273 : break;
274 446 : case AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER:
275 : #if AP_RANGEFINDER_ENABLED
276 446 : if (!copter.rangefinder_state.enabled || !copter.rangefinder.has_orientation(ROTATION_PITCH_270)) {
277 2 : check_failed(Check::PARAMETERS, display_failure, failure_template, "no rangefinder");
278 2 : return false;
279 : }
280 : // check if RTL_ALT_M is higher than rangefinder's max range
281 444 : if (copter.mode_rtl.get_altitude_m() > copter.rangefinder.max_distance_orient(ROTATION_PITCH_270)) {
282 0 : check_failed(Check::PARAMETERS, display_failure, failure_template, "RTL_ALT_M above RNGFND_MAX");
283 0 : return false;
284 : }
285 : #else
286 : check_failed(Check::PARAMETERS, display_failure, failure_template, "rangefinder not in firmware");
287 : #endif
288 444 : break;
289 3 : case AC_WPNav::TerrainSource::TERRAIN_FROM_TERRAINDATABASE:
290 : // these checks are done in AP_Arming
291 3 : break;
292 : }
293 : }
294 : #endif
295 :
296 : // check adsb avoidance failsafe
297 : #if HAL_ADSB_ENABLED
298 89987 : if (copter.failsafe.adsb) {
299 6 : check_failed(Check::PARAMETERS, display_failure, "ADSB threat detected");
300 6 : return false;
301 : }
302 : #endif
303 :
304 : // ensure controllers are OK with us arming:
305 89981 : char failure_msg[100] = {};
306 89981 : if (!copter.pos_control->pre_arm_checks("PSC", failure_msg, ARRAY_SIZE(failure_msg))) {
307 1 : check_failed(Check::PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
308 1 : return false;
309 : }
310 89980 : if (!copter.attitude_control->pre_arm_checks("ATC", failure_msg, ARRAY_SIZE(failure_msg))) {
311 0 : check_failed(Check::PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
312 0 : return false;
313 : }
314 : }
315 :
316 92707 : return true;
317 : }
318 :
319 92716 : bool AP_Arming_Copter::oa_checks(bool display_failure)
320 : {
321 : #if AP_OAPATHPLANNER_ENABLED
322 92716 : char failure_msg[100] = {};
323 92716 : if (copter.g2.oa.pre_arm_check(failure_msg, ARRAY_SIZE(failure_msg))) {
324 92715 : return true;
325 : }
326 : // display failure
327 1 : if (strlen(failure_msg) == 0) {
328 0 : check_failed(display_failure, "%s", "Check Object Avoidance");
329 : } else {
330 1 : check_failed(display_failure, "%s", failure_msg);
331 : }
332 1 : return false;
333 : #else
334 : return true;
335 : #endif
336 : }
337 :
338 90863 : bool AP_Arming_Copter::rc_calibration_checks(bool display_failure)
339 : {
340 : const RC_Channel *channels[] = {
341 90863 : copter.channel_roll,
342 90863 : copter.channel_pitch,
343 90863 : copter.channel_throttle,
344 90863 : copter.channel_yaw
345 90863 : };
346 :
347 : // bitwise & ensures all checks are run
348 90863 : copter.ap.pre_arm_rc_check = rc_checks_copter_sub(display_failure, channels)
349 90863 : & AP_Arming::rc_calibration_checks(display_failure);
350 :
351 90863 : return copter.ap.pre_arm_rc_check;
352 : }
353 :
354 : // performs pre_arm gps related checks and returns true if passed
355 92716 : bool AP_Arming_Copter::gps_checks(bool display_failure)
356 : {
357 : // check if fence requires position
358 92716 : bool fence_requires_position = false;
359 : #if AP_FENCE_ENABLED
360 : // if circular or polygon fence is enabled we need position
361 92716 : fence_requires_position = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0;
362 : #endif
363 :
364 : // check if flight mode requires GPS
365 92716 : const bool mode_requires_position = copter.flightmode->requires_position() || fence_requires_position || (copter.simple_mode == Copter::SimpleMode::SUPERSIMPLE);
366 92716 : const bool mode_requires_gps = AP::ahrs().active_backend_configured_to_use_gps() && mode_requires_position;
367 :
368 : // call parent gps checks
369 92716 : if (mode_requires_gps) {
370 8514 : if (!AP_Arming::gps_checks(display_failure)) {
371 2167 : AP_Notify::flags.pre_arm_gps_check = false;
372 2167 : return false;
373 : }
374 : }
375 :
376 : // run mandatory position checks first
377 90549 : if (!mandatory_position_checks(display_failure)) {
378 5979 : AP_Notify::flags.pre_arm_gps_check = false;
379 5979 : return false;
380 : }
381 :
382 : // return true if GPS is not required
383 84570 : if (!mode_requires_gps) {
384 81810 : AP_Notify::flags.pre_arm_gps_check = true;
385 81810 : return true;
386 : }
387 :
388 : // return true immediately if gps check is disabled
389 2760 : if (!check_enabled(Check::GPS)) {
390 0 : AP_Notify::flags.pre_arm_gps_check = true;
391 0 : return true;
392 : }
393 :
394 : // warn about hdop separately - to prevent user confusion with no gps lock
395 2760 : if ((copter.gps.num_sensors() > 0) && (copter.gps.get_hdop() > copter.g.gps_hdop_good)) {
396 0 : check_failed(Check::GPS, display_failure, "High GPS HDOP");
397 0 : AP_Notify::flags.pre_arm_gps_check = false;
398 0 : return false;
399 : }
400 :
401 : // if we got here all must be ok
402 2760 : AP_Notify::flags.pre_arm_gps_check = true;
403 2760 : return true;
404 : }
405 :
406 : // check ekf attitude is acceptable
407 89989 : bool AP_Arming_Copter::pre_arm_ekf_attitude_check()
408 : {
409 89989 : return AP::ahrs().has_status(AP_AHRS::Status::ATTITUDE_VALID);
410 : }
411 :
412 : #if HAL_PROXIMITY_ENABLED
413 : // check nothing is too close to vehicle
414 92716 : bool AP_Arming_Copter::proximity_checks(bool display_failure) const
415 : {
416 :
417 92716 : if (!AP_Arming::proximity_checks(display_failure)) {
418 121 : return false;
419 : }
420 :
421 92595 : if (!check_enabled(Check::PARAMETERS)) {
422 : // check is disabled
423 2727 : return true;
424 : }
425 :
426 : // get closest object if we might use it for avoidance
427 : #if AP_AVOIDANCE_ENABLED
428 : float angle_deg, distance;
429 89868 : if (copter.avoid.proximity_avoidance_enabled() && copter.g2.proximity.get_closest_object(angle_deg, distance)) {
430 : // display error if something is within 60cm
431 264 : const float tolerance = 0.6f;
432 264 : if (distance <= tolerance) {
433 18 : check_failed(Check::PARAMETERS, display_failure, "Proximity %d deg, %4.2fm (want > %0.1fm)", (int)angle_deg, (double)distance, (double)tolerance);
434 18 : return false;
435 : }
436 : }
437 : #endif
438 :
439 89850 : return true;
440 : }
441 : #endif // HAL_PROXIMITY_ENABLED
442 :
443 : // performs mandatory position checks. returns true if passed
444 90571 : bool AP_Arming_Copter::mandatory_position_checks(bool display_failure)
445 : {
446 : // check if flight mode requires position
447 90571 : bool mode_requires_position = copter.flightmode->requires_position();
448 :
449 : // always check if inertial nav has started and is ready
450 90571 : const auto &ahrs = AP::ahrs();
451 90571 : char failure_msg[100] = {};
452 90571 : if (!ahrs.pre_arm_check(mode_requires_position, failure_msg, sizeof(failure_msg))) {
453 2392 : check_failed(display_failure, "AHRS: %s", failure_msg);
454 2392 : return false;
455 : }
456 :
457 : // check if fence requires position estimate
458 88179 : bool fence_requires_position = false;
459 : #if AP_FENCE_ENABLED
460 : // if circular or polygon fence is enabled we need position estimate
461 88179 : fence_requires_position = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0;
462 : #endif
463 :
464 88179 : if (mode_requires_position || require_location == RequireLocation::YES) {
465 6345 : if (!copter.position_ok()) {
466 : // vehicle level position estimate checks
467 3557 : check_failed(display_failure, "Need Position Estimate");
468 3557 : return false;
469 : }
470 81834 : } else if (fence_requires_position) {
471 31 : if (!copter.position_ok()) {
472 : // clarify to user why they need GPS in non-GPS flight mode
473 24 : check_failed(display_failure, "Fence enabled, need position estimate");
474 24 : return false;
475 : }
476 : } else {
477 : // return true if GPS is not required
478 81803 : return true;
479 : }
480 :
481 : // check for GPS glitch (as reported by EKF)
482 : // this can only be true if the EKF is using the GPS
483 : nav_filter_status filt_status;
484 2795 : if (ahrs.get_filter_status(filt_status)) {
485 2795 : if (filt_status.flags.gps_glitching) {
486 0 : check_failed(display_failure, "GPS glitching");
487 0 : return false;
488 : }
489 : }
490 :
491 : // check EKF's compass, position, height and velocity variances are below failsafe threshold
492 2795 : if (copter.g.fs_ekf_thresh > 0.0f) {
493 : float vel_variance, pos_variance, hgt_variance, tas_variance;
494 2795 : Vector3f mag_variance;
495 2795 : ahrs.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance);
496 : const struct {
497 : const char *name;
498 : float value;
499 2795 : } variances[] {
500 2795 : { "compass", mag_variance.length() },
501 : { "position", pos_variance },
502 : { "velocity", vel_variance },
503 : { "height", hgt_variance },
504 2795 : };
505 13953 : for (auto &variance : variances) {
506 11164 : if (variance.value < copter.g.fs_ekf_thresh) {
507 11158 : continue;
508 : }
509 6 : check_failed(display_failure, "EKF %s variance", variance.name);
510 6 : return false;
511 : }
512 : }
513 :
514 : // if we got here all must be ok
515 2789 : return true;
516 : }
517 :
518 : // Check GCS failsafe
519 92716 : bool AP_Arming_Copter::gcs_failsafe_check(bool display_failure)
520 : {
521 92716 : if (copter.failsafe.gcs) {
522 8 : check_failed(display_failure, "GCS failsafe on");
523 8 : return false;
524 : }
525 92708 : return true;
526 : }
527 :
528 : // check winch
529 92716 : bool AP_Arming_Copter::winch_checks(bool display_failure) const
530 : {
531 : #if AP_WINCH_ENABLED
532 : // pass if parameter or all arming checks disabled
533 92716 : if (!check_enabled(Check::PARAMETERS)) {
534 2727 : return true;
535 : }
536 :
537 89989 : const AP_Winch *winch = AP::winch();
538 89989 : if (winch == nullptr) {
539 0 : return true;
540 : }
541 89989 : char failure_msg[100] = {};
542 89989 : if (!winch->pre_arm_check(failure_msg, sizeof(failure_msg))) {
543 2 : check_failed(display_failure, "%s", failure_msg);
544 2 : return false;
545 : }
546 : #endif
547 89987 : return true;
548 : }
549 :
550 : // performs altitude checks. returns true if passed
551 92738 : bool AP_Arming_Copter::alt_checks(bool display_failure)
552 : {
553 : // always EKF altitude estimate
554 92738 : if (!copter.flightmode->has_manual_throttle() && !copter.ekf_alt_ok()) {
555 228 : check_failed(display_failure, "Need Alt Estimate");
556 228 : return false;
557 : }
558 :
559 92510 : return true;
560 : }
561 :
562 : // arm_checks - perform final checks before arming
563 : // always called just before arming. Return true if ok to arm
564 : // has side-effect that logging is started
565 565 : bool AP_Arming_Copter::arm_checks(AP_Arming::Method method)
566 : {
567 565 : const auto &ahrs = AP::ahrs();
568 :
569 : // always check if inertial nav has started and is ready
570 565 : if (!ahrs.healthy()) {
571 0 : check_failed(true, "AHRS not healthy");
572 0 : return false;
573 : }
574 :
575 : // if non-compass is source of heading we can skip compass health check
576 1128 : if (check_enabled(Check::COMPASS) &&
577 563 : !ahrs.using_noncompass_for_yaw()) {
578 556 : const Compass &_compass = AP::compass();
579 : // check compass health
580 556 : if (!_compass.healthy()) {
581 0 : check_failed(true, "Compass %d not healthy", _compass.get_first_usable() + 1);
582 0 : return false;
583 : }
584 : }
585 :
586 : // always check if the current mode allows arming
587 565 : if (!copter.flightmode->allows_arming(method)) {
588 13 : check_failed(true, "%s mode not armable", copter.flightmode->name());
589 13 : return false;
590 : }
591 :
592 : // succeed if arming checks are disabled
593 552 : if (should_skip_all_checks()) {
594 1 : return true;
595 : }
596 :
597 : // check lean angle
598 551 : if (check_enabled(Check::INS)) {
599 547 : if (acosf(ahrs.cos_roll()*ahrs.cos_pitch()) > copter.attitude_control->lean_angle_max_rad()) {
600 0 : check_failed(Check::INS, true, "Leaning");
601 0 : return false;
602 : }
603 : }
604 :
605 : // check adsb
606 : #if HAL_ADSB_ENABLED
607 551 : if (check_enabled(Check::PARAMETERS)) {
608 547 : if (copter.failsafe.adsb) {
609 0 : check_failed(Check::PARAMETERS, true, "ADSB threat detected");
610 0 : return false;
611 : }
612 : }
613 : #endif
614 :
615 : // check throttle
616 551 : if (check_enabled(Check::RC)) {
617 : #if FRAME_CONFIG == HELI_FRAME
618 59 : const char *rc_item = "Collective";
619 : #else
620 488 : const char *rc_item = "Throttle";
621 : #endif
622 : // check throttle is not too high - skips checks if arming from GCS/scripting in Guided,Guided_NoGPS or Auto
623 547 : if (!((AP_Arming::method_is_GCS(method) || method == AP_Arming::Method::SCRIPTING) && copter.flightmode->allows_GCS_or_SCR_arming_with_throttle_high())) {
624 : // above top of deadband is too always high
625 349 : if (copter.get_pilot_desired_climb_rate_ms() > 0.0f) {
626 4 : check_failed(Check::RC, true, "%s too high", rc_item);
627 4 : return false;
628 : }
629 : // in manual modes throttle must be at zero
630 : #if FRAME_CONFIG == HELI_FRAME
631 52 : if ((copter.flightmode->has_manual_throttle() || copter.flightmode->mode_number() == Mode::Number::DRIFT) && copter.motors->get_takeoff_collective()) {
632 : #else
633 293 : if ((copter.flightmode->has_manual_throttle() || copter.flightmode->mode_number() == Mode::Number::DRIFT) && copter.channel_throttle->get_control_in() > 0) {
634 : #endif
635 0 : check_failed(Check::RC, true, "%s too high", rc_item);
636 0 : return false;
637 : }
638 : }
639 : }
640 :
641 : // check if safety switch has been pushed
642 547 : if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
643 0 : check_failed(true, "Safety Switch");
644 0 : return false;
645 : }
646 :
647 : // superclass method should always be the last thing called; it
648 : // has side-effects which would need to be cleaned up if one of
649 : // our arm checks failed
650 547 : return AP_Arming::arm_checks(method);
651 : }
652 :
653 : // mandatory checks that will be run if ARMING_SKIPCHK skips all or arming forced
654 22 : bool AP_Arming_Copter::mandatory_checks(bool display_failure)
655 : {
656 : // call mandatory position checks and update notify status because regular position checks will not run
657 22 : bool result = mandatory_position_checks(display_failure);
658 22 : AP_Notify::flags.pre_arm_gps_check = result;
659 :
660 : // call mandatory alt check
661 22 : if (!alt_checks(display_failure)) {
662 2 : result = false;
663 : }
664 :
665 22 : return result & AP_Arming::mandatory_checks(display_failure);
666 : }
667 :
668 142354 : void AP_Arming_Copter::set_pre_arm_check(bool b)
669 : {
670 142354 : copter.ap.pre_arm_check = b;
671 142354 : AP_Notify::flags.pre_arm_check = b;
672 142354 : }
673 :
674 630 : bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_checks)
675 : {
676 : static bool in_arm_motors = false;
677 :
678 : // exit immediately if already in this function
679 630 : if (in_arm_motors) {
680 0 : return false;
681 : }
682 630 : in_arm_motors = true;
683 :
684 : // return true if already armed
685 630 : if (copter.motors->armed()) {
686 14 : in_arm_motors = false;
687 14 : return true;
688 : }
689 :
690 616 : if (!AP_Arming::arm(method, do_arming_checks)) {
691 64 : AP_Notify::events.arming_failed = true;
692 64 : in_arm_motors = false;
693 64 : return false;
694 : }
695 :
696 : #if HAL_LOGGING_ENABLED
697 : // let logger know that we're armed (it may open logs e.g.)
698 552 : AP::logger().set_vehicle_armed(true);
699 : #endif
700 :
701 : // disable cpu failsafe because initialising everything takes a while
702 552 : copter.failsafe_disable();
703 :
704 : // notify that arming will occur (we do this early to give plenty of warning)
705 552 : AP_Notify::flags.armed = true;
706 : // call notify update a few times to ensure the message gets out
707 6624 : for (uint8_t i=0; i<=10; i++) {
708 6072 : AP::notify().update();
709 : }
710 :
711 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
712 552 : send_arm_disarm_statustext("Arming motors");
713 : #endif
714 :
715 : // Remember Orientation
716 : // --------------------
717 552 : copter.init_simple_bearing();
718 :
719 552 : auto &ahrs = AP::ahrs();
720 :
721 552 : copter.initial_armed_bearing_rad = ahrs.get_yaw_rad();
722 :
723 552 : if (!ahrs.home_is_set()) {
724 : // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
725 8 : ahrs.resetHeightDatum();
726 8 : LOGGER_WRITE_EVENT(LogEvent::EKF_ALT_RESET);
727 :
728 : // we have reset height, so arming height is zero
729 8 : copter.arming_altitude_m = 0;
730 544 : } else if (!ahrs.home_is_locked()) {
731 : // Reset home position if it has already been set before (but not locked)
732 521 : if (!copter.set_home_to_current_location(false)) {
733 : // ignore failure
734 : }
735 :
736 : // remember the height when we armed (ignore failures)
737 521 : float pos_d_m = 0;
738 521 : UNUSED_RESULT(AP::ahrs().get_relative_position_D_origin_float(pos_d_m));
739 521 : copter.arming_altitude_m = -pos_d_m;
740 : }
741 552 : copter.update_super_simple_bearing(false);
742 :
743 : // Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point
744 : #if MODE_SMARTRTL_ENABLED
745 552 : copter.g2.smart_rtl.set_home(copter.position_ok());
746 : #endif
747 :
748 552 : hal.util->set_soft_armed(true);
749 :
750 : #if HAL_SPRAYER_ENABLED
751 : // turn off sprayer's test if on
752 552 : copter.sprayer.test_pump(false);
753 : #endif
754 :
755 : // output lowest possible value to motors
756 552 : copter.motors->output_min();
757 :
758 : // finally actually arm the motors
759 552 : copter.motors->armed(true);
760 :
761 : #if HAL_LOGGING_ENABLED
762 : // log flight mode in case it was changed while vehicle was disarmed
763 552 : AP::logger().Write_Mode((uint8_t)copter.flightmode->mode_number(), copter.control_mode_reason);
764 : #endif
765 :
766 : // re-enable failsafe
767 552 : copter.failsafe_enable();
768 :
769 : // perf monitor ignores delay due to arming
770 552 : AP::scheduler().perf_info.ignore_this_loop();
771 :
772 : // flag exiting this function
773 552 : in_arm_motors = false;
774 :
775 : // Log time stamp of arming event
776 552 : copter.arm_time_ms = millis();
777 :
778 : // Start the arming delay
779 552 : copter.ap.in_arming_delay = true;
780 :
781 : // assumed armed without a arming, switch. Overridden in switches.cpp
782 552 : copter.ap.armed_with_airmode_switch = false;
783 :
784 : // return success
785 552 : return true;
786 : }
787 :
788 : // arming.disarm - disarm motors
789 648 : bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_checks)
790 : {
791 : // return immediately if we are already disarmed
792 648 : if (!copter.motors->armed()) {
793 74 : return true;
794 : }
795 :
796 : // do not allow disarm via mavlink if we think we are flying:
797 523 : if (do_disarm_checks &&
798 1097 : AP_Arming::method_is_GCS(method) &&
799 74 : !copter.ap.land_complete) {
800 0 : return false;
801 : }
802 :
803 574 : if (method == AP_Arming::Method::RUDDER) {
804 27 : if (!copter.flightmode->has_manual_throttle() && !copter.ap.land_complete) {
805 20 : return false;
806 : }
807 : }
808 :
809 554 : if (!AP_Arming::disarm(method, do_disarm_checks)) {
810 4 : return false;
811 : }
812 :
813 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
814 550 : send_arm_disarm_statustext("Disarming motors");
815 : #endif
816 :
817 : // we are not in the air
818 550 : copter.set_land_complete(true);
819 550 : copter.set_land_complete_maybe(true);
820 :
821 : // send disarm command to motors
822 550 : copter.motors->armed(false);
823 :
824 : #if MODE_AUTO_ENABLED
825 : // reset the mission
826 550 : copter.mode_auto.mission.reset();
827 : #endif
828 :
829 : #if HAL_LOGGING_ENABLED
830 550 : AP::logger().set_vehicle_armed(false);
831 : #endif
832 :
833 550 : hal.util->set_soft_armed(false);
834 :
835 550 : copter.ap.in_arming_delay = false;
836 :
837 : #if AUTOTUNE_ENABLED
838 : // Possibly save auto tuned parameters
839 550 : copter.mode_autotune.autotune.disarmed(copter.flightmode == &copter.mode_autotune);
840 : #endif
841 :
842 550 : return true;
843 : }
844 :
845 : #pragma GCC diagnostic pop
|
