From b1aba9874da62fadd4af0118df8bd667c281266e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 19 Jun 2024 13:34:37 +1000 Subject: [PATCH 1/2] ArduCopter: add Copter flight option for requiring position to arm this is not quite the same as requiring a good position to arm. --- ArduCopter/AP_Arming.cpp | 2 +- ArduCopter/Copter.h | 1 + ArduCopter/Parameters.cpp | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index e233180d34983..9af1d32a4d8ab 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -444,7 +444,7 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure) fence_requires_gps = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0; #endif - if (mode_requires_gps) { + if (mode_requires_gps || copter.option_is_enabled(Copter::FlightOption::REQUIRE_POSITION_FOR_ARMING)) { if (!copter.position_ok()) { // vehicle level position estimate checks check_failed(display_failure, "Need Position Estimate"); diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index eded72ca43b40..f9421c605e9e2 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -626,6 +626,7 @@ class Copter : public AP_Vehicle { DISABLE_THRUST_LOSS_CHECK = (1<<0), // 1 DISABLE_YAW_IMBALANCE_WARNING = (1<<1), // 2 RELEASE_GRIPPER_ON_THRUST_LOSS = (1<<2), // 4 + REQUIRE_POSITION_FOR_ARMING = (1<<3), // 8 }; // returns true if option is enabled for this vehicle bool option_is_enabled(FlightOption option) const { diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 72c8b01f48de6..4b1f79833b756 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1000,7 +1000,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Param: FLIGHT_OPTIONS // @DisplayName: Flight mode options // @Description: Flight mode specific options - // @Bitmask: 0:Disable thrust loss check, 1:Disable yaw imbalance warning, 2:Release gripper on thrust loss + // @Bitmask: 0:Disable thrust loss check, 1:Disable yaw imbalance warning, 2:Release gripper on thrust loss, 3:Require position for arming // @User: Advanced AP_GROUPINFO("FLIGHT_OPTIONS", 44, ParametersG2, flight_options, 0), From 8927b2827362f6f316ab76cea302ef5e19d009e4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 19 Jun 2024 13:34:37 +1000 Subject: [PATCH 2/2] Tools: add Copter flight option for requiring position to arm --- Tools/autotest/arducopter.py | 34 ++++++++++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index f6f8e7a73b9cf..16e1bdfc1195c 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -11535,6 +11535,39 @@ def GripperReleaseOnThrustLoss(self): self.do_RTL() self.reboot_sitl() + def assert_home_position_not_set(self): + try: + self.poll_home_position() + except NotAchievedException: + return + + # if home.lng != 0: etc + + raise NotAchievedException("Home is set when it shouldn't be") + + def REQUIRE_POSITION_FOR_ARMING(self): + '''check FlightOption::REQUIRE_POSITION_FOR_ARMING works''' + self.context_push() + self.set_parameters({ + "SIM_GPS_NUMSATS": 3, # EKF does not like < 6 + }) + self.reboot_sitl() + self.change_mode('STABILIZE') + self.wait_prearm_sys_status_healthy() + self.assert_home_position_not_set() + self.arm_vehicle() + self.disarm_vehicle() + self.change_mode('LOITER') + self.assert_prearm_failure("waiting for home", other_prearm_failures_fatal=False) + + self.change_mode('STABILIZE') + self.set_parameters({ + "FLIGHT_OPTIONS": 8, + }) + self.assert_prearm_failure("Need Position Estimate", other_prearm_failures_fatal=False) + self.context_pop() + self.reboot_sitl() + def tests2b(self): # this block currently around 9.5mins here '''return list of all tests''' ret = ([ @@ -11626,6 +11659,7 @@ def tests2b(self): # this block currently around 9.5mins here self.GuidedWeatherVane, self.Clamp, self.GripperReleaseOnThrustLoss, + self.REQUIRE_POSITION_FOR_ARMING, ]) return ret