From 7a0569fd8f84dbfa4809ed2e62711789d8e4ff88 Mon Sep 17 00:00:00 2001 From: Peter Mullen Date: Fri, 15 Mar 2024 22:17:17 -0700 Subject: [PATCH] autotest: test ArduSub surface tracking modes --- .../SimTerrainMission/terrain_mission.txt | 17 ++ Tools/autotest/ardusub.py | 153 ++++++++++++++++++ 2 files changed, 170 insertions(+) create mode 100644 Tools/autotest/ArduSub_Tests/SimTerrainMission/terrain_mission.txt diff --git a/Tools/autotest/ArduSub_Tests/SimTerrainMission/terrain_mission.txt b/Tools/autotest/ArduSub_Tests/SimTerrainMission/terrain_mission.txt new file mode 100644 index 0000000000..d921705ade --- /dev/null +++ b/Tools/autotest/ArduSub_Tests/SimTerrainMission/terrain_mission.txt @@ -0,0 +1,17 @@ +QGC WPL 110 + +# Drive in a square over the synthetic ocean bottom +# Item 0 is the home position, frame=3 +0 0 3 16 0.000000 0.000000 0.000000 0.000000 33.809780 -118.393982 0.0 1 + +# Sub has already descended to 15m above sea floor. Start mission at 15 and maintain that height, frame=10 +1 0 10 16 0.000000 0.000000 0.000000 0.000000 33.809780 -118.394232 15.0 1 + +# Travel at current rangefinder reading (15m), frame=10 +2 0 10 16 0.000000 0.000000 0.000000 0.000000 33.809780 -118.394432 15.0 1 + +# Travel at current range finder reading, frame=10 +3 0 10 16 0.000000 0.000000 0.000000 0.000000 33.809250 -118.394432 15.0 1 + +# Travel at current range finder reading, frame=10 +4 0 10 16 0.000000 0.000000 0.000000 0.000000 33.809250 -118.394232 15.0 1 diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 6adc6dcc95..bca38d192b 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -271,6 +271,157 @@ class AutoTestSub(vehicle_test_suite.TestSuite): self.disarm_vehicle() self.context_pop() + def prepare_synthetic_seafloor_test(self, sea_floor_depth): + self.set_parameters({ + "SCR_ENABLE": 1, + "RNGFND1_TYPE": 36, + "RNGFND1_ORIENT": 25, + "RNGFND1_MIN_CM": 10, + "RNGFND1_MAX_CM": 3000, + "SCR_USER1": 2, # Configuration bundle + "SCR_USER2": sea_floor_depth, # Depth in meters + "SCR_USER3": 101, # Output log records + }) + + self.install_example_script_context("sub_test_synthetic_seafloor.lua") + + # Reboot to enable scripting. + self.reboot_sitl() + self.set_rc_default() + self.wait_ready_to_arm() + + def watch_true_distance_maintained(self, match_distance, delta=0.3, timeout=5.0, final_waypoint=0): + """Watch and wait for the rangefinder reading to be maintained""" + + def get_true_distance(): + """Return the True distance from the simulated range finder""" + m_true = self.mav.recv_match(type='STATUSTEXT', blocking=True, timeout=3.0) + if m_true is None: + return m_true + idx_tr = m_true.text.find('#TR#') + if idx_tr < 0: + return None + return float(m_true.text[(idx_tr+4):(idx_tr+12)]) + + tstart = self.get_sim_time_cached() + self.progress('Distance to be watched: %.2f (+/- %.2f)' % (match_distance, delta)) + max_delta = 0.0 + + while True: + timed_out = self.get_sim_time_cached() - tstart > timeout + # If final_waypoint>0 then timeout is failure, otherwise success + if timed_out and final_waypoint > 0: + raise NotAchievedException( + "Mission not complete: want waypoint %i, only made it to waypoint %i" % + (final_waypoint, self.mav.waypoint_current())) + if timed_out: + self.progress('Distance hold done. Max delta:%.2fm' % max_delta) + return + + true_distance = get_true_distance() + if true_distance is None: + continue + match_delta = abs(true_distance - match_distance) + if match_delta > max_delta: + max_delta = match_delta + if match_delta > delta: + raise NotAchievedException( + "Distance not maintained: want %.2f (+/- %.2f) got=%.2f (%.2f)" % + (match_distance, delta, true_distance, match_delta)) + if final_waypoint > 0: + if self.mav.waypoint_current() >= final_waypoint: + self.progress('Distance hold during mission done. Max delta:%.2fm' % max_delta) + return + + def SimTerrainSurftrak(self): + """Move at a constant height above synthetic sea floor""" + + sea_floor_depth = 50 # Depth of sea floor at location of test + match_distance = 15 # Desired sub distance from sea floor + start_altitude = -sea_floor_depth+match_distance + end_altitude = start_altitude - 10 + validation_delta = 1.5 # Largest allowed distance between sub height and desired height + + self.context_push() + self.prepare_synthetic_seafloor_test(sea_floor_depth) + self.change_mode('MANUAL') + self.arm_vehicle() + + # Dive to match_distance off the bottom in preparation for the mission + pwm = 1300 if self.get_altitude(relative=True) > start_altitude else 1700 + self.set_rc(Joystick.Throttle, pwm) + self.wait_altitude(altitude_min=start_altitude-1, altitude_max=start_altitude, relative=False, timeout=120) + self.set_rc(Joystick.Throttle, 1500) + self.delay_sim_time(1) + + # Turn on surftrak and move around + self.change_mode(21) + + # Go south over the ridge. + self.reach_heading_manual(180) + self.set_rc(Joystick.Forward, 1650) + self.watch_true_distance_maintained(match_distance, delta=validation_delta, timeout=60) + self.set_rc(Joystick.Forward, 1500) + + # Shift west a bit + self.reach_heading_manual(270) + self.set_rc(Joystick.Forward, 1650) + self.watch_true_distance_maintained(match_distance, delta=validation_delta, timeout=5) + self.set_rc(Joystick.Forward, 1500) + + # Go south over the plateau + self.reach_heading_manual(180) + self.set_rc(Joystick.Forward, 1650) + self.watch_true_distance_maintained(match_distance, delta=validation_delta, timeout=60) + + # The mission ends at end_altitude. Do a check to insure that the sub is at this altitude + self.wait_altitude(altitude_min=end_altitude-validation_delta/2, altitude_max=end_altitude+validation_delta/2, + relative=False, timeout=1) + + self.set_rc(Joystick.Forward, 1500) + + self.disarm_vehicle() + self.context_pop() + + def SimTerrainMission(self): + """Mission at a constant height above synthetic sea floor""" + + sea_floor_depth = 50 # Depth of sea floor at location of test + match_distance = 15 # Desired sub distance from sea floor + start_altitude = -sea_floor_depth+match_distance + end_altitude = start_altitude - 10 + validation_delta = 1.5 # Largest allowed distance between sub height and desired height + + self.context_push() + self.prepare_synthetic_seafloor_test(sea_floor_depth) + + # The synthetic seafloor has an east-west ridge south of the sub. + # The mission contained in terrain_mission.txt instructs the sub + # to remain at 15m above the seafloor and travel south over the + # ridge. Then the sub moves west and travels north over the ridge. + filename = "terrain_mission.txt" + self.load_mission(filename) + + self.change_mode('MANUAL') + self.arm_vehicle() + + # Dive to match_distance off the bottom in preparation for the mission + pwm = 1300 if self.get_altitude(relative=True) > start_altitude else 1700 + self.set_rc(Joystick.Throttle, pwm) + self.wait_altitude(altitude_min=start_altitude-1, altitude_max=start_altitude, relative=False, timeout=120) + self.set_rc(Joystick.Throttle, 1500) + self.delay_sim_time(1) + + self.change_mode('AUTO') + self.watch_true_distance_maintained(match_distance, delta=validation_delta, timeout=500.0, final_waypoint=4) + + # The mission ends at end_altitude. Do a check to insure that the sub is at this altitude. + self.wait_altitude(altitude_min=end_altitude-validation_delta/2, altitude_max=end_altitude+validation_delta/2, + relative=False, timeout=1) + + self.disarm_vehicle() + self.context_pop() + def ModeChanges(self, delta=0.2): """Check if alternating between ALTHOLD, STABILIZE, POSHOLD and SURFTRAK (mode 21) affects altitude""" self.wait_ready_to_arm() @@ -676,6 +827,8 @@ class AutoTestSub(vehicle_test_suite.TestSuite): self.DiveManual, self.AltitudeHold, self.Surftrak, + self.SimTerrainSurftrak, + self.SimTerrainMission, self.RngfndQuality, self.PositionHold, self.ModeChanges,