diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 8b46a62473..2d968b900d 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -8304,6 +8304,57 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.do_RTL() + def TestTetherStuck(self): + """Test tethered vehicle stuck because of tether""" + # Enable tether simulation + self.set_parameters({ + "SIM_TETH_ENABLE": 1, + }) + self.delay_sim_time(2) + self.reboot_sitl() + + # Set tether line length + self.set_parameters({ + "SIM_TETH_LINELEN": 10, + }) + self.delay_sim_time(2) + + # Prepare and take off + self.wait_ready_to_arm() + self.arm_vehicle() + self.takeoff(10, mode='LOITER') + + # Simulate vehicle getting stuck by increasing RC throttle + self.set_rc(3, 1900) + self.delay_sim_time(5, reason='let tether get stuck') + + # Monitor behavior for 10 seconds + tstart = self.get_sim_time() + initial_alt = self.get_altitude() + stuck = True # Assume it's stuck unless proven otherwise + + while self.get_sim_time() - tstart < 10: + # Fetch current altitude + current_alt = self.get_altitude() + self.progress(f"current_alt={current_alt}") + + # Fetch and log battery status + battery_status = self.mav.recv_match(type='BATTERY_STATUS', blocking=True, timeout=1) + if battery_status: + self.progress(f"Battery: {battery_status}") + + # Check if the vehicle is stuck. + # We assume the vehicle is stuck if the current is high and the altitude is not changing + if battery_status and (battery_status.current_battery < 6500 or abs(current_alt - initial_alt) > 2): + stuck = False # Vehicle moved or current is abnormal + break + + if not stuck: + raise NotAchievedException("Vehicle did not get stuck as expected") + + # Land and disarm the vehicle to ensure we can go down + self.land_and_disarm() + def fly_rangefinder_drivers_fly(self, rangefinders): '''ensure rangefinder gives height-above-ground''' self.change_mode('GUIDED') @@ -12330,6 +12381,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.MAV_CMD_MISSION_START_p1_p2, self.ScriptingAHRSSource, self.CommonOrigin, + self.TestTetherStuck, ]) return ret