From 9700e65d190f66c07f7f87bb001fb50381557e0e Mon Sep 17 00:00:00 2001 From: Ep Pravitra Date: Fri, 8 Sep 2023 01:01:23 +0700 Subject: [PATCH] AP_AdvancedFailsafe: option to automatically set mode to auto when datalink is loss When enabled, it ensures vehicle enters AUTO even if it is currently in other autonomous modes (for example Guided) --- ArduCopter/afs_copter.cpp | 5 +++++ ArduCopter/afs_copter.h | 3 +++ ArduPlane/afs_plane.cpp | 7 +++++++ ArduPlane/afs_plane.h | 3 +++ Rover/afs_rover.cpp | 5 +++++ Rover/afs_rover.h | 3 +++ libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp | 4 ++++ libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h | 4 ++++ 8 files changed, 34 insertions(+) diff --git a/ArduCopter/afs_copter.cpp b/ArduCopter/afs_copter.cpp index 9f669eee8c..3bad987a34 100644 --- a/ArduCopter/afs_copter.cpp +++ b/ArduCopter/afs_copter.cpp @@ -68,4 +68,9 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Copter::afs_mode(void) return AP_AdvancedFailsafe::AFS_STABILIZED; } +//to force entering auto mode when datalink loss + void AP_AdvancedFailsafe_Copter::set_mode_auto(void) + { + copter.set_mode(Mode::Number::AUTO,ModeReason::GCS_FAILSAFE); + } #endif // ADVANCED_FAILSAFE diff --git a/ArduCopter/afs_copter.h b/ArduCopter/afs_copter.h index fd73351973..b7c8bc8b21 100644 --- a/ArduCopter/afs_copter.h +++ b/ArduCopter/afs_copter.h @@ -39,6 +39,9 @@ protected: // return the AFS mapped control mode enum control_mode afs_mode(void) override; + + //to force entering auto mode when datalink loss + void set_mode_auto(void) override; }; #endif // ADVANCED_FAILSAFE diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp index 2900d6e767..298d7f5fe8 100644 --- a/ArduPlane/afs_plane.cpp +++ b/ArduPlane/afs_plane.cpp @@ -103,4 +103,11 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Plane::afs_mode(void) } return AP_AdvancedFailsafe::AFS_STABILIZED; } + +//to force entering auto mode when datalink loss + void AP_AdvancedFailsafe_Plane::set_mode_auto(void) + { + plane.set_mode(plane.mode_auto,ModeReason::GCS_FAILSAFE); + } + #endif // AP_ADVANCEDFAILSAFE_ENABLED diff --git a/ArduPlane/afs_plane.h b/ArduPlane/afs_plane.h index 142de7f345..ffd341481c 100644 --- a/ArduPlane/afs_plane.h +++ b/ArduPlane/afs_plane.h @@ -39,6 +39,9 @@ protected: // return the AFS mapped control mode enum control_mode afs_mode(void) override; + + //to force entering auto mode when datalink loss + void set_mode_auto(void) override; }; #endif // AP_ADVANCEDFAILSAFE_ENABLED diff --git a/Rover/afs_rover.cpp b/Rover/afs_rover.cpp index e0349eefc0..4265a20887 100644 --- a/Rover/afs_rover.cpp +++ b/Rover/afs_rover.cpp @@ -29,4 +29,9 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Rover::afs_mode(void) return AP_AdvancedFailsafe::AFS_STABILIZED; } +//to force entering auto mode when datalink loss + void AP_AdvancedFailsafe_Rover::set_mode_auto(void) + { + over.set_mode(rover.mode_auto,ModeReason::GCS_FAILSAFE); + } #endif // ADVANCED_FAILSAFE diff --git a/Rover/afs_rover.h b/Rover/afs_rover.h index 36dd0a0aa9..aa3473ddc3 100644 --- a/Rover/afs_rover.h +++ b/Rover/afs_rover.h @@ -38,6 +38,9 @@ protected: // return the AFS mapped control mode enum control_mode afs_mode(void) override; + + //to force entering auto mode when datalink loss + void set_mode_auto(void) override; }; #endif // ADVANCED_FAILSAFE diff --git a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp index b382e34dca..f7101004cb 100644 --- a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp +++ b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp @@ -167,6 +167,7 @@ const AP_Param::GroupInfo AP_AdvancedFailsafe::var_info[] = { // @DisplayName: AFS options // @Description: See description for each bitmask bit description // @Bitmask: 0: Continue the mission even after comms are recovered (does not go to the mission item at the time comms were lost) + // @Bitmask: 1: Enable AFS for all autonomous modes (not just AUTO) AP_GROUPINFO("OPTIONS", 21, AP_AdvancedFailsafe, options, 0), AP_GROUPEND @@ -246,6 +247,9 @@ AP_AdvancedFailsafe::check(uint32_t last_valid_rc_ms) if (_wp_comms_hold) { _saved_wp = mission.get_current_nav_cmd().index; mission.set_current_cmd(_wp_comms_hold); + if (mode == AFS_AUTO && option_is_set(Option::GCS_FS_ALL_AUTONOMOUS_MODES)) { + set_mode_auto(); + } } // if two events happen within 30s we consider it to be part of the same event if (now - _last_comms_loss_ms > 30*1000UL) { diff --git a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h index caf1fbe888..d66730f679 100644 --- a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h +++ b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h @@ -105,6 +105,9 @@ protected: // return the AFS mapped control mode virtual enum control_mode afs_mode(void) = 0; + //to force entering auto mode when datalink loss + virtual void set_mode_auto(void) = 0; + enum state _state; AP_Int8 _enable; @@ -166,6 +169,7 @@ private: AP_Int16 options; enum class Option { CONTINUE_AFTER_RECOVERED = (1U<<0), + GCS_FS_ALL_AUTONOMOUS_MODES = (1U<<1), }; bool option_is_set(Option option) const { return (options.get() & int16_t(option)) != 0;