From 5f17e33b39b9a6e592e217707f86df7369a67b4b Mon Sep 17 00:00:00 2001 From: arshPratap Date: Mon, 24 Jul 2023 14:59:19 +0530 Subject: [PATCH] AP_Arming: Added DDS Method for Arming/Disarming --- ArduCopter/AP_Arming.cpp | 4 ++-- ArduCopter/mode_guided.cpp | 2 +- ArduPlane/AP_Arming.cpp | 2 +- libraries/AP_Arming/AP_Arming.cpp | 1 + libraries/AP_Arming/AP_Arming.h | 4 ++++ 5 files changed, 9 insertions(+), 4 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index a51085530b..1bdbc630bc 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -623,7 +623,7 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method) const char *rc_item = "Throttle"; #endif // check throttle is not too high - skips checks if arming from GCS/scripting in Guided,Guided_NoGPS or Auto - if (!((method == AP_Arming::Method::MAVLINK || method == AP_Arming::Method::SCRIPTING) && (copter.flightmode->mode_number() == Mode::Number::GUIDED || copter.flightmode->mode_number() == Mode::Number::GUIDED_NOGPS || copter.flightmode->mode_number() == Mode::Number::AUTO))) { + if (!((AP_Arming::method_is_GCS(method) || method == AP_Arming::Method::SCRIPTING) && (copter.flightmode->mode_number() == Mode::Number::GUIDED || copter.flightmode->mode_number() == Mode::Number::GUIDED_NOGPS || copter.flightmode->mode_number() == Mode::Number::AUTO))) { // above top of deadband is too always high if (copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in()) > 0.0f) { check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item); @@ -790,7 +790,7 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che // do not allow disarm via mavlink if we think we are flying: if (do_disarm_checks && - method == AP_Arming::Method::MAVLINK && + AP_Arming::method_is_GCS(method) && !copter.ap.land_complete) { return false; } diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index fd313a1c0d..b69bb84b2e 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -100,7 +100,7 @@ void ModeGuided::run() bool ModeGuided::allows_arming(AP_Arming::Method method) const { // always allow arming from the ground station or scripting - if (method == AP_Arming::Method::MAVLINK || method == AP_Arming::Method::SCRIPTING) { + if (AP_Arming::method_is_GCS(method) || method == AP_Arming::Method::SCRIPTING) { return true; } diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 7be7455207..7ab5e92b8e 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -325,7 +325,7 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_checks) { if (do_disarm_checks && - (method == AP_Arming::Method::MAVLINK || + (AP_Arming::method_is_GCS(method) || method == AP_Arming::Method::RUDDER)) { if (plane.is_flying()) { // don't allow mavlink or rudder disarm while flying diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 6ec165ab84..2bd9814b32 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -1832,6 +1832,7 @@ void AP_Arming::check_forced_logging(const AP_Arming::Method method) case Method::TOYMODELANDTHROTTLE: case Method::TOYMODELANDFORCE: case Method::LANDING: + case Method::DDS: case Method::UNKNOWN: AP::logger().set_long_log_persist(false); return; diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h index 0423f94383..a59aad14bd 100644 --- a/libraries/AP_Arming/AP_Arming.h +++ b/libraries/AP_Arming/AP_Arming.h @@ -78,6 +78,7 @@ public: LANDING = 32, // only disarm uses this... DEADRECKON_FAILSAFE = 33, // only disarm uses this... BLACKBOX = 34, + DDS = 35, UNKNOWN = 100, }; @@ -144,6 +145,9 @@ public: return (_arming_options & uint32_t(option)) != 0; } + static bool method_is_GCS(Method method) { + return (method == Method::MAVLINK || method == Method::DDS); + } protected: // Parameters