autotest: add test for conversion of command long to command int sani…

…ty checks
ArduPilot · Dec 15, 2024 · 7abba85 · 7abba85
1 parent 0b4cee8
commit 7abba85
Show file tree

Hide file tree

Showing 2 changed files with 66 additions and 0 deletions.
diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py
@@ -6945,6 +6945,61 @@ def SDPolyFence(self):
 
         self.delay_sim_time(1000)
 
+    def convert_COMMAND_LONG_to_COMMAND_INT(self):
+        '''test the convert_COMMAND_LONG_to_COMMAND_INT function'''
+        self.change_mode('GUIDED')
+        self.wait_ready_to_arm()
+        self.run_cmd(
+            mavutil.mavlink.MAV_CMD_DO_SET_HOME,
+            p5=0,  # lat
+            p6=0,  # lng
+            p7=0,  # alt
+        )
+
+        self.run_cmd(
+            mavutil.mavlink.MAV_CMD_DO_SET_HOME,
+            p5=10,  # lat
+            p6=20,  # lng
+            p7=30,  # alt
+        )
+        self.assert_home_position_at(10, 20, 30)
+
+        self.progress("Ensure NaN is bounced")
+        self.run_cmd(
+            mavutil.mavlink.MAV_CMD_DO_SET_HOME,
+            p5=float("NaN"),  # lat
+            p6=10,  # lng
+            p7=10,  # alt
+            want_result=mavutil.mavlink.MAV_RESULT_DENIED,
+        )
+
+        self.progress("Ensure INF is bounced")
+        self.run_cmd(
+            mavutil.mavlink.MAV_CMD_DO_SET_HOME,
+            p5=float("Inf"),  # lat
+            p6=10,  # lng
+            p7=10,  # alt
+            want_result=mavutil.mavlink.MAV_RESULT_DENIED,
+        )
+
+        self.progress("Ensure OOB lat is bounced")
+        self.run_cmd(
+            mavutil.mavlink.MAV_CMD_DO_SET_HOME,
+            p5=91,  # lat
+            p6=10,  # lng
+            p7=10,  # alt
+            want_result=mavutil.mavlink.MAV_RESULT_DENIED,
+        )
+
+        self.progress("Ensure OOB lng is bounced")
+        self.run_cmd(
+            mavutil.mavlink.MAV_CMD_DO_SET_HOME,
+            p5=10,  # lat
+            p6=190,  # lng
+            p7=10,  # alt
+            want_result=mavutil.mavlink.MAV_RESULT_DENIED,
+        )
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestRover, self).tests()
@@ -7043,6 +7098,7 @@ def tests(self):
             self.ClearMission,
             self.JammingSimulation,
             self.BatteryInvalid,
+            self.convert_COMMAND_LONG_to_COMMAND_INT,
         ])
         return ret
 

diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py
@@ -9418,6 +9418,16 @@ def home_position_as_mav_location(self):
         m = self.poll_home_position()
         return mavutil.location(m.latitude*1.0e-7, m.longitude*1.0e-7, m.altitude*1.0e-3, 0)
 
+    def assert_home_position_at(self, lat, lng, alt):
+        '''assert home is at specified lat/lng/alt, deg/deg/metres'''
+        home = self.home_position_as_mav_location()
+        if home.lat != lat:
+            raise NotAchievedException("Bad lat")
+        if home.lng != lng:
+            raise NotAchievedException("Bad lng")
+        if home.alt != alt:
+            raise NotAchievedException("Bad alt")
+
     def offset_location_ne(self, location, metres_north, metres_east):
         '''return a new location offset from passed-in location'''
         (target_lat, target_lng) = mavextra.gps_offset(location.lat,