8 pre_arm_display_counter++;
9 bool display_fail =
false;
12 pre_arm_display_counter = 0;
31 if (
copter.motors->armed()) {
38 if (display_failure) {
39 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: Interlock/E-Stop Conflict");
48 if (
copter.ap.using_interlock &&
copter.ap.motor_interlock_switch) {
49 if (display_failure) {
50 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: Motor Interlock Enabled");
82 if (display_failure) {
83 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: Altitude disparity");
100 if (display_failure) {
101 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: Compass not calibrated");
112 #if AC_FENCE == ENABLED 114 const char *fail_msg =
nullptr;
115 if (!
copter.fence.pre_arm_check(fail_msg)) {
116 if (display_failure && fail_msg !=
nullptr) {
117 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: %s", fail_msg);
133 if (display_failure) {
134 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: gyros still settling");
151 if (
copter.battery.has_failsafed()) {
152 if (display_failure) {
153 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: Battery failsafe");
173 if (
copter.check_duplicate_auxsw()) {
174 if (display_failure) {
175 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: Duplicate Aux Switch Options");
181 if (
copter.g.failsafe_throttle) {
183 if (
copter.channel_throttle->get_radio_min() <=
copter.g.failsafe_throttle_value+10 ||
copter.g.failsafe_throttle_value < 910) {
184 if (display_failure) {
185 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: Check FS_THR_VALUE");
192 if (
copter.aparm.angle_max < 1000 ||
copter.aparm.angle_max > 8000) {
193 if (display_failure) {
194 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: Check ANGLE_MAX");
200 #if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED 201 if ((
copter.g.acro_balance_roll >
copter.attitude_control->get_angle_roll_p().kP()) || (
copter.g.acro_balance_pitch >
copter.attitude_control->get_angle_pitch_p().kP())) {
202 if (display_failure) {
203 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: ACRO_BAL_ROLL/PITCH");
209 #if RANGEFINDER_ENABLED == ENABLED && OPTFLOW == ENABLED 211 if (
copter.optflow.enabled() && !
copter.rangefinder.pre_arm_check()) {
212 if (display_failure) {
213 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: check range finder");
219 #if FRAME_CONFIG == HELI_FRAME 221 if (!
copter.motors->parameter_check(display_failure)) {
226 if (display_failure) {
227 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: Inverted flight option not supported");
239 #if ADSB_ENABLED == ENABLE 240 if (
copter.failsafe.adsb) {
241 if (display_failure) {
242 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: ADSB threat detected");
258 }
else if (
is_zero(
copter.pos_control->get_pos_z_p().kP())) {
261 }
else if (
is_zero(
copter.pos_control->get_vel_z_p().kP())) {
264 }
else if (
is_zero(
copter.pos_control->get_accel_z_pid().kP())) {
267 }
else if (
is_zero(
copter.pos_control->get_accel_z_pid().kI())) {
270 }
else if (
is_zero(
copter.attitude_control->get_rate_roll_pid().kP()) &&
is_zero(
copter.attitude_control->get_rate_roll_pid().ff())) {
273 }
else if (
is_zero(
copter.attitude_control->get_rate_roll_pid().kI()) &&
is_zero(
copter.attitude_control->get_rate_roll_pid().ff())) {
276 }
else if (
is_zero(
copter.attitude_control->get_rate_roll_pid().kD()) &&
is_zero(
copter.attitude_control->get_rate_roll_pid().ff())) {
279 }
else if (
is_zero(
copter.attitude_control->get_rate_pitch_pid().kP()) &&
is_zero(
copter.attitude_control->get_rate_pitch_pid().ff())) {
282 }
else if (
is_zero(
copter.attitude_control->get_rate_pitch_pid().kI()) &&
is_zero(
copter.attitude_control->get_rate_pitch_pid().ff())) {
285 }
else if (
is_zero(
copter.attitude_control->get_rate_pitch_pid().kD()) &&
is_zero(
copter.attitude_control->get_rate_pitch_pid().ff())) {
288 }
else if (
is_zero(
copter.attitude_control->get_rate_yaw_pid().kP()) &&
is_zero(
copter.attitude_control->get_rate_yaw_pid().ff())) {
291 }
else if (
is_zero(
copter.attitude_control->get_rate_yaw_pid().kI()) &&
is_zero(
copter.attitude_control->get_rate_yaw_pid().ff())) {
294 }
else if (
is_zero(
copter.attitude_control->get_angle_pitch_p().kP())) {
297 }
else if (
is_zero(
copter.attitude_control->get_angle_roll_p().kP())) {
300 }
else if (
is_zero(
copter.attitude_control->get_angle_yaw_p().kP())) {
311 if (display_failure) {
312 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: Check PIDs - %s", error_msg);
320 if (!
copter.motors->initialised_ok()) {
321 if (display_failure) {
322 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: check firmware or FRAME_CLASS");
335 if (display_failure) {
336 #if FRAME_CONFIG == HELI_FRAME 337 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: Collective below Failsafe");
339 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: Throttle below Failsafe");
361 return copter.ap.pre_arm_rc_check;
369 if (display_failure) {
370 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: Waiting for Nav Checks");
376 bool mode_requires_gps =
copter.flightmode->requires_GPS();
379 bool fence_requires_gps =
false;
380 #if AC_FENCE == ENABLED 386 if (!mode_requires_gps && !fence_requires_gps) {
392 if (!
copter.position_ok()) {
393 if (display_failure) {
396 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: %s", reason);
398 if (!mode_requires_gps && fence_requires_gps) {
400 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: Fence enabled, need 3D Fix");
402 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: Need 3D Fix");
414 if (display_failure) {
415 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: GPS glitching");
422 float vel_variance, pos_variance, hgt_variance, tas_variance;
427 if (display_failure) {
428 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: EKF compass variance");
435 if (display_failure) {
436 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: EKF-home variance");
450 if (display_failure) {
451 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: High GPS HDOP");
480 #if AP_TERRAIN_AVAILABLE && AC_TERRAIN 482 if (!
copter.terrain_use()) {
490 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: RTL_ALT above rangefinder max range");
495 uint16_t terr_pending, terr_loaded;
496 copter.terrain.get_statistics(terr_pending, terr_loaded);
497 bool have_all_data = (terr_pending <= 0);
498 if (!have_all_data && display_failure) {
499 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: Waiting for Terrain data");
501 return have_all_data;
510 #if PROXIMITY_ENABLED == ENABLED 519 if (display_failure) {
520 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: check proximity sensor");
526 #if AC_AVOID_ENABLED == ENABLED 528 if (
copter.avoid.proximity_avoidance_enabled() &&
copter.g2.proximity.get_closest_object(angle_deg, distance)) {
530 if (distance <= 0.6
f) {
531 if (display_failure) {
532 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"PreArm: Proximity %d deg, %4.2fm", (
int)angle_deg, (
double)distance);
552 if (display_failure) {
553 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"Arm: Waiting for Nav Checks");
558 #ifndef ALLOW_ARM_NO_COMPASS 561 if (display_failure) {
562 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"Arm: Compass not healthy");
569 if (display_failure) {
570 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"Arm: Compass calibration running");
577 if (display_failure) {
578 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"Arm: Compass calibrated requires reboot");
586 if (!
copter.flightmode->allows_arming(arming_from_gcs)) {
587 if (display_failure) {
588 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"Arm: Mode not armable");
600 if (
copter.ap.using_interlock &&
copter.ap.motor_interlock_switch) {
601 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"Arm: Motor Interlock Enabled");
608 copter.set_motor_emergency_stop(
false);
611 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"Arm: Motor Emergency Stopped");
623 if (display_failure) {
631 #if ADSB_ENABLED == ENABLE 633 if (
copter.failsafe.adsb) {
634 if (display_failure) {
635 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"Arm: ADSB threat detected");
646 if (display_failure) {
647 #if FRAME_CONFIG == HELI_FRAME 648 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"Arm: Collective below Failsafe");
650 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"Arm: Throttle below Failsafe");
659 if (
copter.get_pilot_desired_climb_rate(
copter.channel_throttle->get_control_in()) > 0.0
f) {
660 if (display_failure) {
661 #if FRAME_CONFIG == HELI_FRAME 662 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"Arm: Collective too high");
664 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"Arm: Throttle too high");
670 if ((
copter.flightmode->has_manual_throttle() || control_mode ==
DRIFT) &&
copter.channel_throttle->get_control_in() > 0) {
671 if (display_failure) {
672 #if FRAME_CONFIG == HELI_FRAME 673 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"Arm: Collective too high");
675 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"Arm: Throttle too high");
685 if (display_failure) {
686 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"Arm: Safety Switch");
const AP_AHRS_NavEKF & _ahrs_navekf
bool arm_checks(bool display_failure, bool arming_from_gcs)
const AP_InertialNav_NavEKF & _inav
bool barometer_checks(bool display_failure) override
float get_altitude() const
bool board_voltage_checks(bool display_failure) override
bool get_filter_status(nav_filter_status &status) const
struct _USB_OTG_GOTGCTL_TypeDef::@51 b
#define AC_FENCE_TYPE_CIRCLE
const struct Location & get_home(void) const
bool all_checks_passing(bool arming_from_gcs)
virtual bool barometer_checks(bool report)
virtual const char * prearm_failure_reason(void) const
uint32_t pre_arm_gps_check
bool configured(uint8_t i)
bool rc_calibration_checks(bool display_failure) override
virtual bool ins_checks(bool report)
bool parameter_checks(bool display_failure)
bool ins_checks(bool display_failure) override
nav_filter_status get_filter_status() const
bool pilot_throttle_checks(bool display_failure)
bool is_zero(const T fVal1)
bool pre_arm_proximity_check(bool display_failure)
AP_Int16 checks_to_perform
bool pre_arm_terrain_check(bool display_failure)
bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const override
void set_pre_arm_check(bool b)
virtual bool pre_arm_checks(bool report)
virtual enum safety_state safety_switch_state(void)
bool healthy[COMPASS_MAX_INSTANCES]
void send_text(MAV_SEVERITY severity, const char *fmt,...)
#define PREARM_DISPLAY_PERIOD
bool pre_arm_checks(bool display_failure) override
virtual bool gps_checks(bool report)
bool battery_checks(bool report)
struct nav_filter_status::@138 flags
bool motor_checks(bool display_failure)
virtual bool rc_calibration_checks(bool report)
bool fence_checks(bool display_failure)
static struct notify_flags_and_values_type flags
virtual bool healthy(void) const=0
#define PREARM_MAX_ALT_DISPARITY_CM
AP_HAL_MAIN_CALLBACKS & copter
bool arm_checks(uint8_t method)
virtual bool compass_checks(bool report)
bool is_calibrating() const
#define AC_FENCE_TYPE_POLYGON
bool pre_arm_ekf_attitude_check()
void parameter_checks_pid_warning_message(bool display_failure, const char *error_msg)
bool compass_checks(bool display_failure) override
bool gps_checks(bool display_failure) override
virtual bool board_voltage_checks(bool report)
bool compass_cal_requires_reboot()
bool rc_checks_copter_sub(bool display_failure, const RC_Channel *channels[4], const bool check_min_max=true) const