diff --git a/ArduSub/AP_State.cpp b/ArduSub/AP_State.cpp index 40590877ec..6185f5a04b 100644 --- a/ArduSub/AP_State.cpp +++ b/ArduSub/AP_State.cpp @@ -37,16 +37,6 @@ void Sub::set_auto_armed(bool b) } } -// --------------------------------------------- - -void Sub::set_pre_arm_check(bool b) -{ - if (ap.pre_arm_check != b) { - ap.pre_arm_check = b; - AP_Notify::flags.pre_arm_check = b; - } -} - void Sub::set_motor_emergency_stop(bool b) { if (ap.motor_emergency_stop != b) { diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 20197cfb0a..3acb3efffb 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -363,7 +363,9 @@ void Sub::three_hz_loop() // one_hz_loop - runs at 1Hz void Sub::one_hz_loop() { - AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false); + bool arm_check = arming.pre_arm_checks(false); + ap.pre_arm_check = arm_check; + AP_Notify::flags.pre_arm_check = arm_check; AP_Notify::flags.pre_arm_gps_check = position_ok(); if (should_log(MASK_LOG_ANY)) { diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 849819a77c..50facb8145 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -470,7 +470,6 @@ private: void set_home_state(enum HomeState new_home_state); bool home_is_set(); void set_auto_armed(bool b); - void set_pre_arm_check(bool b); void set_motor_emergency_stop(bool b); float get_smoothing_gain(); void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max);