autotest: added Plane.TerrainRally test

reproduces the issue from
ArduPilot#25157

Tools/autotest/arduplane.py

@@ -1836,6 +1836,85 @@ def FenceRetRally(self):
         self.do_fence_disable() # Disable fence so we can land
         self.fly_home_land_and_disarm() # Pack it up, we're going home.
 
+    def TerrainRally(self):
+        """ Tests terrain follow with a rally point """
+        self.context_push()
+        self.install_terrain_handlers_context()
+        self.progress("Testing FENCE_ACTION_RTL with fence rally point")
+
+        def terrain_following_above_80m(mav, m):
+            if m.get_type() == 'TERRAIN_REPORT':
+                if m.current_height < 50:
+                    raise NotAchievedException(
+                        "TERRAIN_REPORT.current_height below 50m %fm" % m.current_height)
+            if m.get_type() == 'VFR_HUD':
+                if m.groundspeed < 2:
+                    raise NotAchievedException("hit ground")
+
+        def terrain_wait_path(loc1, loc2, steps):
+            '''wait till we have terrain for N steps from loc1 to loc2'''
+            tstart = self.get_sim_time_cached()
+            self.progress("Waiting for terrain data")
+            while True:
+                now = self.get_sim_time_cached()
+                if now - tstart > 60:
+                    raise NotAchievedException("Did not get correct required terrain")
+                for i in range(steps):
+                    lat = loc1.lat + i * (loc2.lat-loc1.lat)/steps
+                    lon = loc1.lng + i * (loc2.lng-loc1.lng)/steps
+                    self.mav.mav.terrain_check_send(int(lat*1.0e7), int(lon*1.0e7))
+
+                report = self.mav.recv_match(type='TERRAIN_REPORT', blocking=True, timeout=60)
+                self.progress("Terrain pending=%u" % report.pending)
+                if report.pending == 0:
+                    break
+            self.progress("Got required terrain")
+
+        self.wait_ready_to_arm()
+        self.homeloc = self.mav.location()
+
+        guided_loc = mavutil.location(-35.39723762, 149.07284612, 99.0, 0)
+        rally_loc = mavutil.location(-35.3654952000, 149.1558698000, 100, 0)
+
+        terrain_wait_path(self.homeloc, rally_loc, 10)
+
+        # set a rally point to the west of home
+        self.set_parameter("RALLY_TOTAL", 1)
+        self.add_rally_point(rally_loc, 0, 1)
+        self.delay_sim_time(1)
+
+        self.set_parameter("TKOFF_ALT", 100)
+        self.change_mode("TAKEOFF")
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.set_parameter("TERRAIN_FOLLOW", 1)
+        self.wait_altitude(90, 120, timeout=30, relative=True)
+        self.progress("Done takeoff")
+
+        self.install_message_hook_context(terrain_following_above_80m)
+
+        self.change_mode("GUIDED")
+        self.do_reposition(guided_loc, frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT)
+        self.progress("Flying to guided location")
+        self.wait_location(guided_loc,
+                           accuracy=200,
+                           target_altitude=None,
+                           timeout=600)
+
+        self.progress("Reached guided location")
+        self.set_parameter("RALLY_LIMIT_KM", 50)
+        self.change_mode("RTL")
+        self.progress("Flying to rally point")
+        self.wait_location(rally_loc,
+                           accuracy=200,
+                           target_altitude=None,
+                           timeout=600)
+        self.progress("Reached rally point")
+
+        self.context_pop()
+        self.disarm_vehicle(force=True)
+        self.reboot_sitl()
+
     def Parachute(self):
         '''Test Parachute'''
         self.set_rc(9, 1000)
@@ -5042,6 +5121,7 @@ def tests(self):
             self.MAV_CMD_DO_AUTOTUNE_ENABLE,
             self.MAV_CMD_DO_GO_AROUND,
             self.MAV_CMD_DO_FLIGHTTERMINATION,
+            self.TerrainRally,
         ])
         return ret
 

Tools/autotest/common.py

@@ -7084,6 +7084,35 @@ def assert_rc_channel_value(self, channel, value):
             raise NotAchievedException("Expected %s to be %u got %u" %
                                        (channel, value, m_value))
 
+    def do_reposition(self,
+                      loc,
+                      frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT):
+        '''send a DO_REPOSITION command for a location'''
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_DO_REPOSITION,
+            0,
+            0,
+            0,
+            0,
+            int(loc.lat*1e7), # lat* 1e7
+            int(loc.lng*1e7), # lon* 1e7
+            loc.alt,
+            frame=frame
+        )
+
+    def add_rally_point(self, loc, seq, total):
+        '''add a rally point at the given location'''
+        self.mav.mav.rally_point_send(1, # target system
+                                      0, # target component
+                                      seq, # sequence number
+                                      total, # total count
+                                      int(loc.lat * 1e7),
+                                      int(loc.lng * 1e7),
+                                      loc.alt, # relative alt
+                                      0, # "break" alt?!
+                                      0, # "land dir"
+                                      0) # flags
+
     def wait_location(self,
                       loc,
                       accuracy=5.0,