forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 1
/
AP_Arming.cpp
2005 lines (1780 loc) · 66.2 KB
/
AP_Arming.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
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
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_Arming.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_Notify/AP_Notify.h>
#include <GCS_MAVLink/GCS.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_Mission/AP_Mission.h>
#include <AP_Proximity/AP_Proximity.h>
#include <AP_Rally/AP_Rally.h>
#include <SRV_Channel/SRV_Channel.h>
#include <AC_Fence/AC_Fence.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_InternalError/AP_InternalError.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_Declination/AP_Declination.h>
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <AP_Generator/AP_Generator.h>
#include <AP_Terrain/AP_Terrain.h>
#include <AP_ADSB/AP_ADSB.h>
#include <AP_Scripting/AP_Scripting.h>
#include <AP_Camera/AP_RunCam.h>
#include <AP_GyroFFT/AP_GyroFFT.h>
#include <AP_RCMapper/AP_RCMapper.h>
#include <AP_VisualOdom/AP_VisualOdom.h>
#include <AP_Parachute/AP_Parachute.h>
#include <AP_OSD/AP_OSD.h>
#include <AP_Relay/AP_Relay.h>
#include <RC_Channel/RC_Channel.h>
#include <AP_Button/AP_Button.h>
#include <AP_FETtecOneWire/AP_FETtecOneWire.h>
#include <AP_RPM/AP_RPM.h>
#include <AP_Mount/AP_Mount.h>
#include <AP_OpenDroneID/AP_OpenDroneID.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_Scheduler/AP_Scheduler.h>
#include <AP_KDECAN/AP_KDECAN.h>
#include <AP_Vehicle/AP_Vehicle.h>
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
#include <AP_CANManager/AP_CANManager.h>
#include <AP_Common/AP_Common.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_PiccoloCAN/AP_PiccoloCAN.h>
#include <AP_DroneCAN/AP_DroneCAN.h>
#endif
#include <AP_Logger/AP_Logger.h>
#define AP_ARMING_COMPASS_MAGFIELD_EXPECTED 530
#define AP_ARMING_COMPASS_MAGFIELD_MIN 185 // 0.35 * 530 milligauss
#define AP_ARMING_COMPASS_MAGFIELD_MAX 875 // 1.65 * 530 milligauss
#define AP_ARMING_BOARD_VOLTAGE_MAX 5.8f
#define AP_ARMING_ACCEL_ERROR_THRESHOLD 0.75f
#define AP_ARMING_MAGFIELD_ERROR_THRESHOLD 100
#define AP_ARMING_AHRS_GPS_ERROR_MAX 10 // accept up to 10m difference between AHRS and GPS
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
#define ARMING_RUDDER_DEFAULT (uint8_t)RudderArming::ARMONLY
#else
#define ARMING_RUDDER_DEFAULT (uint8_t)RudderArming::ARMDISARM
#endif
#ifndef PREARM_DISPLAY_PERIOD
# define PREARM_DISPLAY_PERIOD 30
#endif
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_Arming::var_info[] = {
// @Param{Plane, Rover}: REQUIRE
// @DisplayName: Require Arming Motors
// @Description: Arming disabled until some requirements are met. If 0, there are no requirements (arm immediately). If 1, sends the minimum throttle PWM value to the throttle channel when disarmed. If 2, send 0 PWM (no signal) to throttle channel when disarmed. On planes with ICE enabled and the throttle while disarmed option set in ICE_OPTIONS, the motor will always get THR_MIN when disarmed. Arming will occur using either rudder stick arming (if enabled) or GCS command when all mandatory and ARMING_CHECK items are satisfied. Note, when setting this parameter to 0, a reboot is required to immediately arm the plane.
// @Values: 0:Disabled,1:minimum PWM when disarmed,2:0 PWM when disarmed
// @User: Advanced
AP_GROUPINFO_FLAGS_FRAME("REQUIRE", 0, AP_Arming, require, float(Required::YES_MIN_PWM),
AP_PARAM_FLAG_NO_SHIFT,
AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_ROVER),
// 2 was the CHECK paramter stored in a AP_Int16
// @Param: ACCTHRESH
// @DisplayName: Accelerometer error threshold
// @Description: Accelerometer error threshold used to determine inconsistent accelerometers. Compares this error range to other accelerometers to detect a hardware or calibration error. Lower value means tighter check and harder to pass arming check. Not all accelerometers are created equal.
// @Units: m/s/s
// @Range: 0.25 3.0
// @User: Advanced
AP_GROUPINFO("ACCTHRESH", 3, AP_Arming, accel_error_threshold, AP_ARMING_ACCEL_ERROR_THRESHOLD),
// index 4 was VOLT_MIN, moved to AP_BattMonitor
// index 5 was VOLT2_MIN, moved to AP_BattMonitor
// @Param: RUDDER
// @DisplayName: Arming with Rudder enable/disable
// @Description: Allow arm/disarm by rudder input. When enabled arming can be done with right rudder, disarming with left rudder. Rudder arming only works with throttle at zero +- deadzone (RCx_DZ). Depending on vehicle type, arming in certain modes is prevented. See the wiki for each vehicle. Caution is recommended when arming if it is allowed in an auto-throttle mode!
// @Values: 0:Disabled,1:ArmingOnly,2:ArmOrDisarm
// @User: Advanced
AP_GROUPINFO_FRAME("RUDDER", 6, AP_Arming, _rudder_arming, ARMING_RUDDER_DEFAULT, AP_PARAM_FRAME_PLANE |
AP_PARAM_FRAME_ROVER |
AP_PARAM_FRAME_COPTER |
AP_PARAM_FRAME_TRICOPTER |
AP_PARAM_FRAME_HELI |
AP_PARAM_FRAME_BLIMP),
// @Param: MIS_ITEMS
// @DisplayName: Required mission items
// @Description: Bitmask of mission items that are required to be planned in order to arm the aircraft
// @Bitmask: 0:Land,1:VTOL Land,2:DO_LAND_START,3:Takeoff,4:VTOL Takeoff,5:Rallypoint,6:RTL
// @User: Advanced
AP_GROUPINFO("MIS_ITEMS", 7, AP_Arming, _required_mission_items, 0),
// @Param: CHECK
// @DisplayName: Arm Checks to Perform (bitmask)
// @Description: Checks prior to arming motor. This is a bitmask of checks that will be performed before allowing arming. For most users it is recommended to leave this at the default of 1 (all checks enabled). You can select whatever checks you prefer by adding together the values of each check type to set this parameter. For example, to only allow arming when you have GPS lock and no RC failsafe you would set ARMING_CHECK to 72.
// @Bitmask: 0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC Channels,7:Board voltage,8:Battery Level,10:Logging Available,11:Hardware safety switch,12:GPS Configuration,13:System,14:Mission,15:Rangefinder,16:Camera,17:AuxAuth,18:VisualOdometry,19:FFT
// @Bitmask{Plane}: 0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC Channels,7:Board voltage,8:Battery Level,9:Airspeed,10:Logging Available,11:Hardware safety switch,12:GPS Configuration,13:System,14:Mission,15:Rangefinder,16:Camera,17:AuxAuth,19:FFT
// @User: Standard
AP_GROUPINFO("CHECK", 8, AP_Arming, checks_to_perform, ARMING_CHECK_ALL),
// @Param: OPTIONS
// @DisplayName: Arming options
// @Description: Options that can be applied to change arming behaviour
// @Values: 0:None,1:Disable prearm display,2:Do not send status text on state change
// @User: Advanced
AP_GROUPINFO("OPTIONS", 9, AP_Arming, _arming_options, 0),
// @Param: MAGTHRESH
// @DisplayName: Compass magnetic field strength error threshold vs earth magnetic model
// @Description: Compass magnetic field strength error threshold vs earth magnetic model. X and y axis are compared using this threhold, Z axis uses 2x this threshold. 0 to disable check
// @Units: mGauss
// @Range: 0 500
// @User: Advanced
AP_GROUPINFO("MAGTHRESH", 10, AP_Arming, magfield_error_threshold, AP_ARMING_MAGFIELD_ERROR_THRESHOLD),
AP_GROUPEND
};
#if HAL_WITH_IO_MCU
#include <AP_IOMCU/AP_IOMCU.h>
extern AP_IOMCU iomcu;
#endif
#pragma GCC diagnostic push
#if defined (__clang__)
#pragma GCC diagnostic ignored "-Wbitwise-instead-of-logical"
#endif
AP_Arming::AP_Arming()
{
if (_singleton) {
AP_HAL::panic("Too many AP_Arming instances");
}
_singleton = this;
AP_Param::setup_object_defaults(this, var_info);
}
// performs pre-arm checks. expects to be called at 1hz.
void AP_Arming::update(void)
{
const uint32_t now_ms = AP_HAL::millis();
// perform pre-arm checks & display failures every 30 seconds
bool display_fail = false;
if ((report_immediately && (now_ms - last_prearm_display_ms > 4000)) ||
(now_ms - last_prearm_display_ms > PREARM_DISPLAY_PERIOD*1000)) {
report_immediately = false;
display_fail = true;
last_prearm_display_ms = now_ms;
}
// OTOH, the user may never want to display them:
if (option_enabled(Option::DISABLE_PREARM_DISPLAY)) {
display_fail = false;
}
pre_arm_checks(display_fail);
}
uint16_t AP_Arming::compass_magfield_expected() const
{
return AP_ARMING_COMPASS_MAGFIELD_EXPECTED;
}
bool AP_Arming::is_armed() const
{
return armed || arming_required() == Required::NO;
}
/*
true if armed and safety is off
*/
bool AP_Arming::is_armed_and_safety_off() const
{
return is_armed() && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;
}
uint32_t AP_Arming::get_enabled_checks() const
{
return checks_to_perform;
}
bool AP_Arming::check_enabled(const enum AP_Arming::ArmingChecks check) const
{
if (checks_to_perform & ARMING_CHECK_ALL) {
return true;
}
return (checks_to_perform & check);
}
void AP_Arming::check_failed(const enum AP_Arming::ArmingChecks check, bool report, const char *fmt, ...) const
{
if (!report) {
return;
}
char taggedfmt[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
// metafmt is wrapped around the passed-in format string to
// prepend "PreArm" or "Arm", depending on what sorts of checks
// we're currently doing.
const char *metafmt = "PreArm: %s"; // it's formats all the way down
if (running_arming_checks) {
metafmt = "Arm: %s";
}
hal.util->snprintf(taggedfmt, sizeof(taggedfmt), metafmt, fmt);
#if HAL_GCS_ENABLED
MAV_SEVERITY severity = MAV_SEVERITY_CRITICAL;
if (!check_enabled(check)) {
// technically should be NOTICE, but will annoy users at that level:
severity = MAV_SEVERITY_DEBUG;
}
va_list arg_list;
va_start(arg_list, fmt);
gcs().send_textv(severity, taggedfmt, arg_list);
va_end(arg_list);
#endif // HAL_GCS_ENABLED
}
void AP_Arming::check_failed(bool report, const char *fmt, ...) const
{
#if HAL_GCS_ENABLED
if (!report) {
return;
}
char taggedfmt[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
// metafmt is wrapped around the passed-in format string to
// prepend "PreArm" or "Arm", depending on what sorts of checks
// we're currently doing.
const char *metafmt = "PreArm: %s"; // it's formats all the way down
if (running_arming_checks) {
metafmt = "Arm: %s";
}
hal.util->snprintf(taggedfmt, sizeof(taggedfmt), metafmt, fmt);
va_list arg_list;
va_start(arg_list, fmt);
gcs().send_textv(MAV_SEVERITY_CRITICAL, taggedfmt, arg_list);
va_end(arg_list);
#endif // HAL_GCS_ENABLED
}
bool AP_Arming::barometer_checks(bool report)
{
#ifdef HAL_BARO_ALLOW_INIT_NO_BARO
return true;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (AP::sitl()->baro_count == 0) {
// simulate no baro boards
return true;
}
#endif
if (check_enabled(ARMING_CHECK_BARO)) {
char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1] {};
if (!AP::baro().arming_checks(sizeof(buffer), buffer)) {
check_failed(ARMING_CHECK_BARO, report, "Baro: %s", buffer);
return false;
}
}
return true;
}
#if AP_AIRSPEED_ENABLED
bool AP_Arming::airspeed_checks(bool report)
{
if (check_enabled(ARMING_CHECK_AIRSPEED)) {
const AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
if (airspeed == nullptr) {
// not an airspeed capable vehicle
return true;
}
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
if (airspeed->enabled(i) && airspeed->use(i) && !airspeed->healthy(i)) {
check_failed(ARMING_CHECK_AIRSPEED, report, "Airspeed %d not healthy", i + 1);
return false;
}
}
}
return true;
}
#endif // AP_AIRSPEED_ENABLED
#if HAL_LOGGING_ENABLED
bool AP_Arming::logging_checks(bool report)
{
if (check_enabled(ARMING_CHECK_LOGGING)) {
if (!AP::logger().logging_present()) {
// Logging is disabled, so nothing to check.
return true;
}
if (AP::logger().logging_failed()) {
check_failed(ARMING_CHECK_LOGGING, report, "Logging failed");
return false;
}
if (!AP::logger().CardInserted()) {
check_failed(ARMING_CHECK_LOGGING, report, "No SD card");
return false;
}
if (AP::logger().in_log_download()) {
check_failed(ARMING_CHECK_LOGGING, report, "Downloading logs");
return false;
}
}
return true;
}
#endif // HAL_LOGGING_ENABLED
#if AP_INERTIALSENSOR_ENABLED
bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins)
{
const uint8_t accel_count = ins.get_accel_count();
if (accel_count <= 1) {
return true;
}
const Vector3f &prime_accel_vec = ins.get_accel();
const uint32_t now = AP_HAL::millis();
for(uint8_t i=0; i<accel_count; i++) {
if (!ins.use_accel(i)) {
continue;
}
// get next accel vector
const Vector3f &accel_vec = ins.get_accel(i);
Vector3f vec_diff = accel_vec - prime_accel_vec;
// allow for user-defined difference, typically 0.75 m/s/s. Has to pass in last 10 seconds
float threshold = accel_error_threshold;
if (i >= 2) {
/*
we allow for a higher threshold for IMU3 as it
runs at a different temperature to IMU1/IMU2,
and is not used for accel data in the EKF
*/
threshold *= 3;
}
// EKF is less sensitive to Z-axis error
vec_diff.z *= 0.5f;
if (vec_diff.length() > threshold) {
// this sensor disagrees with the primary sensor, so
// accels are inconsistent:
last_accel_pass_ms = 0;
return false;
}
}
if (last_accel_pass_ms == 0) {
// we didn't return false in the loop above, so sensors are
// consistent right now:
last_accel_pass_ms = now;
}
// must pass for at least 10 seconds before we're considered consistent:
if (now - last_accel_pass_ms < 10000) {
return false;
}
return true;
}
bool AP_Arming::ins_gyros_consistent(const AP_InertialSensor &ins)
{
const uint8_t gyro_count = ins.get_gyro_count();
if (gyro_count <= 1) {
return true;
}
const Vector3f &prime_gyro_vec = ins.get_gyro();
const uint32_t now = AP_HAL::millis();
for(uint8_t i=0; i<gyro_count; i++) {
if (!ins.use_gyro(i)) {
continue;
}
// get next gyro vector
const Vector3f &gyro_vec = ins.get_gyro(i);
const Vector3f vec_diff = gyro_vec - prime_gyro_vec;
// allow for up to 5 degrees/s difference
if (vec_diff.length() > radians(5)) {
// this sensor disagrees with the primary sensor, so
// gyros are inconsistent:
last_gyro_pass_ms = 0;
return false;
}
}
// we didn't return false in the loop above, so sensors are
// consistent right now:
if (last_gyro_pass_ms == 0) {
last_gyro_pass_ms = now;
}
// must pass for at least 10 seconds before we're considered consistent:
if (now - last_gyro_pass_ms < 10000) {
return false;
}
return true;
}
bool AP_Arming::ins_checks(bool report)
{
if (check_enabled(ARMING_CHECK_INS)) {
const AP_InertialSensor &ins = AP::ins();
if (!ins.get_gyro_health_all()) {
check_failed(ARMING_CHECK_INS, report, "Gyros not healthy");
return false;
}
if (!ins.gyro_calibrated_ok_all()) {
check_failed(ARMING_CHECK_INS, report, "Gyros not calibrated");
return false;
}
if (!ins.get_accel_health_all()) {
check_failed(ARMING_CHECK_INS, report, "Accels not healthy");
return false;
}
if (!ins.accel_calibrated_ok_all()) {
check_failed(ARMING_CHECK_INS, report, "3D Accel calibration needed");
return false;
}
//check if accelerometers have calibrated and require reboot
if (ins.accel_cal_requires_reboot()) {
check_failed(ARMING_CHECK_INS, report, "Accels calibrated requires reboot");
return false;
}
// check all accelerometers point in roughly same direction
if (!ins_accels_consistent(ins)) {
check_failed(ARMING_CHECK_INS, report, "Accels inconsistent");
return false;
}
// check all gyros are giving consistent readings
if (!ins_gyros_consistent(ins)) {
check_failed(ARMING_CHECK_INS, report, "Gyros inconsistent");
return false;
}
// no arming while doing temp cal
if (ins.temperature_cal_running()) {
check_failed(ARMING_CHECK_INS, report, "temperature cal running");
return false;
}
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
// If Batch sampling enabled it must be initialized
if (ins.batchsampler.enabled() && !ins.batchsampler.is_initialised()) {
check_failed(ARMING_CHECK_INS, report, "Batch sampling requires reboot");
return false;
}
#endif
}
#if HAL_GYROFFT_ENABLED
// gyros are healthy so check the FFT
if (check_enabled(ARMING_CHECK_FFT)) {
// Check that the noise analyser works
AP_GyroFFT *fft = AP::fft();
char fail_msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
if (fft != nullptr && !fft->pre_arm_check(fail_msg, ARRAY_SIZE(fail_msg))) {
check_failed(ARMING_CHECK_INS, report, "%s", fail_msg);
return false;
}
}
#endif
return true;
}
#endif // AP_INERTIALSENSOR_ENABLED
bool AP_Arming::compass_checks(bool report)
{
Compass &_compass = AP::compass();
#if COMPASS_CAL_ENABLED
// check if compass is calibrating
if (_compass.is_calibrating()) {
check_failed(report, "Compass calibration running");
return false;
}
// check if compass has calibrated and requires reboot
if (_compass.compass_cal_requires_reboot()) {
check_failed(report, "Compass calibrated requires reboot");
return false;
}
#endif
if (check_enabled(ARMING_CHECK_COMPASS)) {
// avoid Compass::use_for_yaw(void) as it implicitly calls healthy() which can
// incorrectly skip the remaining checks, pass the primary instance directly
if (!_compass.use_for_yaw(0)) {
// compass use is disabled
return true;
}
if (!_compass.healthy()) {
check_failed(ARMING_CHECK_COMPASS, report, "Compass not healthy");
return false;
}
// check compass learning is on or offsets have been set
#if !APM_BUILD_COPTER_OR_HELI && !APM_BUILD_TYPE(APM_BUILD_Blimp)
// check compass offsets have been set if learning is off
// copter and blimp always require configured compasses
if (!_compass.learn_offsets_enabled())
#endif
{
char failure_msg[50] = {};
if (!_compass.configured(failure_msg, ARRAY_SIZE(failure_msg))) {
check_failed(ARMING_CHECK_COMPASS, report, "%s", failure_msg);
return false;
}
}
// check for unreasonable compass offsets
const Vector3f offsets = _compass.get_offsets();
if (offsets.length() > _compass.get_offsets_max()) {
check_failed(ARMING_CHECK_COMPASS, report, "Compass offsets too high");
return false;
}
// check for unreasonable mag field length
const float mag_field = _compass.get_field().length();
if (mag_field > AP_ARMING_COMPASS_MAGFIELD_MAX || mag_field < AP_ARMING_COMPASS_MAGFIELD_MIN) {
check_failed(ARMING_CHECK_COMPASS, report, "Check mag field: %4.0f, max %d, min %d", (double)mag_field, AP_ARMING_COMPASS_MAGFIELD_MAX, AP_ARMING_COMPASS_MAGFIELD_MIN);
return false;
}
// check all compasses point in roughly same direction
if (!_compass.consistent()) {
check_failed(ARMING_CHECK_COMPASS, report, "Compasses inconsistent");
return false;
}
#if AP_AHRS_ENABLED
// if ahrs is using compass and we have location, check mag field versus expected earth magnetic model
Location ahrs_loc;
AP_AHRS &ahrs = AP::ahrs();
if ((magfield_error_threshold > 0) && ahrs.use_compass() && ahrs.get_location(ahrs_loc)) {
const Vector3f veh_mag_field_ef = ahrs.get_rotation_body_to_ned() * _compass.get_field();
const Vector3f earth_field_mgauss = AP_Declination::get_earth_field_ga(ahrs_loc) * 1000.0;
const Vector3f diff_mgauss = veh_mag_field_ef - earth_field_mgauss;
if (MAX(fabsf(diff_mgauss.x), fabsf(diff_mgauss.y)) > magfield_error_threshold) {
check_failed(ARMING_CHECK_COMPASS, report, "Check mag field (xy diff:%.0f>%d)",
(double)MAX(fabsf(diff_mgauss.x), (double)fabsf(diff_mgauss.y)), (int)magfield_error_threshold);
return false;
}
if (fabsf(diff_mgauss.x) > magfield_error_threshold*2.0) {
check_failed(ARMING_CHECK_COMPASS, report, "Check mag field (z diff:%.0f>%d)",
(double)fabsf(diff_mgauss.z), (int)magfield_error_threshold*2);
return false;
}
}
#endif // AP_AHRS_ENABLED
}
return true;
}
#if AP_GPS_ENABLED
bool AP_Arming::gps_checks(bool report)
{
const AP_GPS &gps = AP::gps();
if (check_enabled(ARMING_CHECK_GPS)) {
// Any failure messages from GPS backends
char failure_msg[50] = {};
if (!AP::gps().backends_healthy(failure_msg, ARRAY_SIZE(failure_msg))) {
if (failure_msg[0] != '\0') {
check_failed(ARMING_CHECK_GPS, report, "%s", failure_msg);
}
return false;
}
for (uint8_t i = 0; i < gps.num_sensors(); i++) {
#if defined(GPS_BLENDED_INSTANCE)
if ((i != GPS_BLENDED_INSTANCE) &&
#else
if (
#endif
(gps.get_type(i) == AP_GPS::GPS_Type::GPS_TYPE_NONE)) {
if (gps.primary_sensor() == i) {
check_failed(ARMING_CHECK_GPS, report, "GPS %i: primary but TYPE 0", i+1);
return false;
}
continue;
}
//GPS OK?
if (gps.status(i) < AP_GPS::GPS_OK_FIX_3D) {
check_failed(ARMING_CHECK_GPS, report, "GPS %i: Bad fix", i+1);
return false;
}
//GPS update rate acceptable
if (!gps.is_healthy(i)) {
check_failed(ARMING_CHECK_GPS, report, "GPS %i: not healthy", i+1);
return false;
}
}
if (!AP::ahrs().home_is_set()) {
check_failed(ARMING_CHECK_GPS, report, "AHRS: waiting for home");
return false;
}
// check GPSs are within 50m of each other and that blending is healthy
float distance_m;
if (!gps.all_consistent(distance_m)) {
check_failed(ARMING_CHECK_GPS, report, "GPS positions differ by %4.1fm",
(double)distance_m);
return false;
}
#if defined(GPS_BLENDED_INSTANCE)
if (!gps.blend_health_check()) {
check_failed(ARMING_CHECK_GPS, report, "GPS blending unhealthy");
return false;
}
#endif
// check AHRS and GPS are within 10m of each other
if (gps.num_sensors() > 0) {
const Location gps_loc = gps.location();
Location ahrs_loc;
if (AP::ahrs().get_location(ahrs_loc)) {
const float distance = gps_loc.get_distance(ahrs_loc);
if (distance > AP_ARMING_AHRS_GPS_ERROR_MAX) {
check_failed(ARMING_CHECK_GPS, report, "GPS and AHRS differ by %4.1fm", (double)distance);
return false;
}
}
}
}
if (check_enabled(ARMING_CHECK_GPS_CONFIG)) {
uint8_t first_unconfigured;
if (gps.first_unconfigured_gps(first_unconfigured)) {
check_failed(ARMING_CHECK_GPS_CONFIG,
report,
"GPS %d still configuring this GPS",
first_unconfigured + 1);
if (report) {
gps.broadcast_first_configuration_failure_reason();
}
return false;
}
}
return true;
}
#endif // AP_GPS_ENABLED
#if AP_BATTERY_ENABLED
bool AP_Arming::battery_checks(bool report)
{
if (check_enabled(ARMING_CHECK_BATTERY)) {
char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1] {};
if (!AP::battery().arming_checks(sizeof(buffer), buffer)) {
check_failed(ARMING_CHECK_BATTERY, report, "%s", buffer);
return false;
}
}
return true;
}
#endif // AP_BATTERY_ENABLED
bool AP_Arming::hardware_safety_check(bool report)
{
if (check_enabled(ARMING_CHECK_SWITCH)) {
// check if safety switch has been pushed
if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
check_failed(ARMING_CHECK_SWITCH, report, "Hardware safety switch");
return false;
}
}
return true;
}
#if AP_RC_CHANNEL_ENABLED
bool AP_Arming::rc_arm_checks(AP_Arming::Method method)
{
// don't check the trims if we are in a failsafe
if (!rc().has_valid_input()) {
return true;
}
// only check if we've received some form of input within the last second
// this is a protection against a vehicle having never enabled an input
uint32_t last_input_ms = rc().last_input_ms();
if ((last_input_ms == 0) || ((AP_HAL::millis() - last_input_ms) > 1000)) {
return true;
}
bool check_passed = true;
// ensure all rc channels have different functions
if (rc().duplicate_options_exist()) {
check_failed(ARMING_CHECK_PARAMETERS, true, "Duplicate Aux Switch Options");
check_passed = false;
}
if (rc().flight_mode_channel_conflicts_with_rc_option()) {
check_failed(ARMING_CHECK_PARAMETERS, true, "Mode channel and RC%d_OPTION conflict", rc().flight_mode_channel_number());
check_passed = false;
}
const RCMapper * rcmap = AP::rcmap();
if (rcmap != nullptr) {
if (!rc().option_is_enabled(RC_Channels::Option::ARMING_SKIP_CHECK_RPY)) {
const char *names[3] = {"Roll", "Pitch", "Yaw"};
const uint8_t channels[3] = {rcmap->roll(), rcmap->pitch(), rcmap->yaw()};
for (uint8_t i = 0; i < ARRAY_SIZE(channels); i++) {
const RC_Channel *c = rc().channel(channels[i] - 1);
if (c == nullptr) {
continue;
}
if (c->get_control_in() != 0) {
if ((method != Method::RUDDER) || (c != rc().get_arming_channel())) { // ignore the yaw input channel if rudder arming
check_failed(ARMING_CHECK_RC, true, "%s (RC%d) is not neutral", names[i], channels[i]);
check_passed = false;
}
}
}
}
// if throttle check is enabled, require zero input
if (rc().arming_check_throttle()) {
RC_Channel *c = rc().channel(rcmap->throttle() - 1);
if (c != nullptr) {
if (c->get_control_in() != 0) {
check_failed(ARMING_CHECK_RC, true, "Throttle (RC%d) is not neutral", rcmap->throttle());
check_passed = false;
}
}
c = rc().find_channel_for_option(RC_Channel::AUX_FUNC::FWD_THR);
if (c != nullptr) {
uint8_t fwd_thr = c->percent_input();
// require channel input within 2% of minimum
if (fwd_thr > 2) {
check_failed(ARMING_CHECK_RC, true, "VTOL Fwd Throttle is not zero");
check_passed = false;
}
}
}
}
return check_passed;
}
bool AP_Arming::rc_calibration_checks(bool report)
{
bool check_passed = true;
const uint8_t num_channels = RC_Channels::get_valid_channel_count();
for (uint8_t i = 0; i < NUM_RC_CHANNELS; i++) {
const RC_Channel *c = rc().channel(i);
if (c == nullptr) {
continue;
}
if (i >= num_channels && !(c->has_override())) {
continue;
}
const uint16_t trim = c->get_radio_trim();
if (c->get_radio_min() > trim) {
check_failed(ARMING_CHECK_RC, report, "RC%d_MIN is greater than RC%d_TRIM", i + 1, i + 1);
check_passed = false;
}
if (c->get_radio_max() < trim) {
check_failed(ARMING_CHECK_RC, report, "RC%d_MAX is less than RC%d_TRIM", i + 1, i + 1);
check_passed = false;
}
}
return check_passed;
}
bool AP_Arming::rc_in_calibration_check(bool report)
{
if (rc().calibrating()) {
check_failed(ARMING_CHECK_RC, report, "RC calibrating");
return false;
}
return true;
}
bool AP_Arming::manual_transmitter_checks(bool report)
{
if (check_enabled(ARMING_CHECK_RC)) {
if (AP_Notify::flags.failsafe_radio) {
check_failed(ARMING_CHECK_RC, report, "Radio failsafe on");
return false;
}
if (!rc_calibration_checks(report)) {
return false;
}
}
return rc_in_calibration_check(report);
}
#endif // AP_RC_CHANNEL_ENABLED
#if AP_MISSION_ENABLED
bool AP_Arming::mission_checks(bool report)
{
AP_Mission *mission = AP::mission();
if (check_enabled(ARMING_CHECK_MISSION) && _required_mission_items) {
if (mission == nullptr) {
check_failed(ARMING_CHECK_MISSION, report, "No mission library present");
return false;
}
const struct MisItemTable {
MIS_ITEM_CHECK check;
MAV_CMD mis_item_type;
const char *type;
} misChecks[] = {
{MIS_ITEM_CHECK_LAND, MAV_CMD_NAV_LAND, "land"},
{MIS_ITEM_CHECK_VTOL_LAND, MAV_CMD_NAV_VTOL_LAND, "vtol land"},
{MIS_ITEM_CHECK_DO_LAND_START, MAV_CMD_DO_LAND_START, "do land start"},
{MIS_ITEM_CHECK_TAKEOFF, MAV_CMD_NAV_TAKEOFF, "takeoff"},
{MIS_ITEM_CHECK_VTOL_TAKEOFF, MAV_CMD_NAV_VTOL_TAKEOFF, "vtol takeoff"},
{MIS_ITEM_CHECK_RETURN_TO_LAUNCH, MAV_CMD_NAV_RETURN_TO_LAUNCH, "RTL"},
};
for (uint8_t i = 0; i < ARRAY_SIZE(misChecks); i++) {
if (_required_mission_items & misChecks[i].check) {
if (!mission->contains_item(misChecks[i].mis_item_type)) {
check_failed(ARMING_CHECK_MISSION, report, "Missing mission item: %s", misChecks[i].type);
return false;
}
}
}
if (_required_mission_items & MIS_ITEM_CHECK_RALLY) {
#if HAL_RALLY_ENABLED
AP_Rally *rally = AP::rally();
if (rally == nullptr) {
check_failed(ARMING_CHECK_MISSION, report, "No rally library present");
return false;
}
Location ahrs_loc;
if (!AP::ahrs().get_location(ahrs_loc)) {
check_failed(ARMING_CHECK_MISSION, report, "Can't check rally without position");
return false;
}
RallyLocation rally_loc = {};
if (!rally->find_nearest_rally_point(ahrs_loc, rally_loc)) {
check_failed(ARMING_CHECK_MISSION, report, "No sufficiently close rally point located");
return false;
}
#else
check_failed(ARMING_CHECK_MISSION, report, "No rally library present");
return false;
#endif
}
}
#if AP_SDCARD_STORAGE_ENABLED
if (check_enabled(ARMING_CHECK_MISSION) &&
mission != nullptr &&
(mission->failed_sdcard_storage() || StorageManager::storage_failed())) {
check_failed(ARMING_CHECK_MISSION, report, "Failed to open %s", AP_MISSION_SDCARD_FILENAME);
return false;
}
#endif
#if AP_VEHICLE_ENABLED
// do not allow arming if there are no mission items and we are in
// (e.g.) AUTO mode
if (AP::vehicle()->current_mode_requires_mission() &&
(mission == nullptr || mission->num_commands() <= 1)) {
check_failed(ARMING_CHECK_MISSION, report, "Mode requires mission");
return false;
}
#endif
return true;
}
#endif // AP_MISSION_ENABLED
bool AP_Arming::rangefinder_checks(bool report)
{
if (check_enabled(ARMING_CHECK_RANGEFINDER)) {
RangeFinder *range = RangeFinder::get_singleton();
if (range == nullptr) {
return true;
}
char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
if (!range->prearm_healthy(buffer, ARRAY_SIZE(buffer))) {
check_failed(ARMING_CHECK_RANGEFINDER, report, "%s", buffer);
return false;
}
}
return true;
}
bool AP_Arming::servo_checks(bool report) const
{
#if NUM_SERVO_CHANNELS
bool check_passed = true;
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
const SRV_Channel *c = SRV_Channels::srv_channel(i);
if (c == nullptr || c->get_function() <= SRV_Channel::k_none) {
continue;
}
const uint16_t trim = c->get_trim();
if (c->get_output_min() > trim) {
check_failed(report, "SERVO%d_MIN is greater than SERVO%d_TRIM", i + 1, i + 1);
check_passed = false;
}
if (c->get_output_max() < trim) {
check_failed(report, "SERVO%d_MAX is less than SERVO%d_TRIM", i + 1, i + 1);
check_passed = false;
}
// check functions using PWM are enabled
if (SRV_Channels::get_disabled_channel_mask() & 1U<<i) {
const SRV_Channel::Aux_servo_function_t ch_function = c->get_function();
// motors, e-stoppable functions, neopixels and ProfiLEDs may be digital outputs and thus can be disabled
// scripting can use its functions as labels for LED setup
const bool disabled_ok = SRV_Channel::is_motor(ch_function) ||
SRV_Channel::should_e_stop(ch_function) ||
(ch_function >= SRV_Channel::k_LED_neopixel1 && ch_function <= SRV_Channel::k_LED_neopixel4) ||
(ch_function >= SRV_Channel::k_ProfiLED_1 && ch_function <= SRV_Channel::k_ProfiLED_Clock) ||
(ch_function >= SRV_Channel::k_scripting1 && ch_function <= SRV_Channel::k_scripting16);
// for all other functions raise a pre-arm failure
if (!disabled_ok) {
check_failed(report, "SERVO%u_FUNCTION=%u on disabled channel", i + 1, (unsigned)ch_function);
check_passed = false;
}
}
}
#if HAL_WITH_IO_MCU
if (!iomcu.healthy() && AP_BoardConfig::io_enabled()) {
check_failed(report, "IOMCU is unhealthy");
check_passed = false;
}
#endif
return check_passed;
#else
return false;
#endif
}
bool AP_Arming::board_voltage_checks(bool report)
{
// check board voltage
if (check_enabled(ARMING_CHECK_VOLTAGE)) {