diff --git a/ArduSub/AP_Arming_Sub.cpp b/ArduSub/AP_Arming_Sub.cpp index f60a2b905a..197cab95b9 100644 --- a/ArduSub/AP_Arming_Sub.cpp +++ b/ArduSub/AP_Arming_Sub.cpp @@ -155,14 +155,14 @@ bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks) return true; } -bool AP_Arming_Sub::disarm() +bool AP_Arming_Sub::disarm(const AP_Arming::Method method) { // return immediately if we are already disarmed if (!sub.motors.armed()) { return false; } - if (!AP_Arming::disarm()) { + if (!AP_Arming::disarm(method)) { return false; } diff --git a/ArduSub/AP_Arming_Sub.h b/ArduSub/AP_Arming_Sub.h index 2fab5503fb..5906b67aa0 100644 --- a/ArduSub/AP_Arming_Sub.h +++ b/ArduSub/AP_Arming_Sub.h @@ -15,7 +15,7 @@ public: bool pre_arm_checks(bool display_failure) override; bool has_disarm_function() const; - bool disarm() override; + bool disarm(AP_Arming::Method method) override; bool arm(AP_Arming::Method method, bool do_arming_checks=true) override; protected: diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index a180374d20..203296e634 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -794,7 +794,7 @@ void GCS_MAVLINK_Sub::handle_rc_channels_override(const mavlink_message_t &msg) */ MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_long_t &packet) { if (packet.param1 > 0.5f) { - sub.arming.disarm(); + sub.arming.disarm(AP_Arming::Method::TERMINATION); return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; diff --git a/ArduSub/control_motordetect.cpp b/ArduSub/control_motordetect.cpp index 348dba9868..1718de53b4 100644 --- a/ArduSub/control_motordetect.cpp +++ b/ArduSub/control_motordetect.cpp @@ -168,7 +168,7 @@ void Sub::motordetect_run() } case DONE: control_mode = prev_control_mode; - arming.disarm(); + arming.disarm(AP_Arming::Method::MOTORDETECTDONE); break; } } diff --git a/ArduSub/failsafe.cpp b/ArduSub/failsafe.cpp index 856330f850..701fa9aba3 100644 --- a/ArduSub/failsafe.cpp +++ b/ArduSub/failsafe.cpp @@ -92,7 +92,7 @@ void Sub::failsafe_sensors_check() // This should always succeed if (!set_mode(MANUAL, ModeReason::BAD_DEPTH)) { // We should never get here - arming.disarm(); + arming.disarm(AP_Arming::Method::BADFLOWOFCONTROL); } } } @@ -146,7 +146,7 @@ void Sub::failsafe_ekf_check() } if (g.fs_ekf_action == FS_EKF_ACTION_DISARM) { - arming.disarm(); + arming.disarm(AP_Arming::Method::EKFFAILSAFE); } } @@ -160,7 +160,7 @@ void Sub::handle_battery_failsafe(const char* type_str, const int8_t action) set_mode(SURFACE, ModeReason::BATTERY_FAILSAFE); break; case Failsafe_Action_Disarm: - arming.disarm(); + arming.disarm(AP_Arming::Method::BATTERYFAILSAFE); break; case Failsafe_Action_Warn: case Failsafe_Action_None: @@ -194,7 +194,7 @@ void Sub::failsafe_pilot_input_check() set_neutral_controls(); if(g.failsafe_pilot_input == FS_PILOT_INPUT_DISARM) { - arming.disarm(); + arming.disarm(AP_Arming::Method::PILOT_INPUT_FAILSAFE); } #endif } @@ -345,14 +345,14 @@ void Sub::failsafe_gcs_check() // handle failsafe action if (g.failsafe_gcs == FS_GCS_DISARM) { - arming.disarm(); + arming.disarm(AP_Arming::Method::GCSFAILSAFE); } else if (g.failsafe_gcs == FS_GCS_HOLD && motors.armed()) { if (!set_mode(ALT_HOLD, ModeReason::GCS_FAILSAFE)) { - arming.disarm(); + arming.disarm(AP_Arming::Method::GCS_FAILSAFE_HOLDFAILED); } } else if (g.failsafe_gcs == FS_GCS_SURFACE && motors.armed()) { if (!set_mode(SURFACE, ModeReason::GCS_FAILSAFE)) { - arming.disarm(); + arming.disarm(AP_Arming::Method::GCS_FAILSAFE_SURFACEFAILED); } } } @@ -411,7 +411,7 @@ void Sub::failsafe_crash_check() // disarm motors if (g.fs_crash_check == FS_CRASH_DISARM) { - arming.disarm(); + arming.disarm(AP_Arming::Method::CRASH); } } @@ -490,6 +490,6 @@ void Sub::failsafe_terrain_act() case FS_TERRAIN_DISARM: default: - arming.disarm(); + arming.disarm(AP_Arming::Method::TERRAINFAILSAFE); } } diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index f1f14b1dee..d9c72d3ac0 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -145,7 +145,7 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) switch (get_button(_button)->function(shift)) { case JSButton::button_function_t::k_arm_toggle: if (motors.armed()) { - arming.disarm(); + arming.disarm(AP_Arming::Method::MAVLINK); } else { arming.arm(AP_Arming::Method::MAVLINK); } @@ -154,7 +154,7 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) arming.arm(AP_Arming::Method::MAVLINK); break; case JSButton::button_function_t::k_disarm: - arming.disarm(); + arming.disarm(AP_Arming::Method::MAVLINK); break; case JSButton::button_function_t::k_mode_manual: diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h index 027655cda1..68bfadaad2 100644 --- a/libraries/AP_Arming/AP_Arming.h +++ b/libraries/AP_Arming/AP_Arming.h @@ -66,6 +66,7 @@ public: GCS_FAILSAFE_HOLDFAILED = 26, // only disarm uses this... TAKEOFFTIMEOUT = 27, // only disarm uses this... AUTOLANDED = 28, // only disarm uses this... + PILOT_INPUT_FAILSAFE = 29, // only disarm uses this... }; enum class Required {