autotest: Rebase conflict resolution

Originating from ArduPilot#28030

Tools/autotest/arduplane.py

@@ -4372,8 +4372,8 @@ def LandingDrift(self):
         self.wait_disarmed(timeout=180)
 
     def TakeoffAuto1(self):
-        '''Test the behaviour of an AUTO takeoff, pt1.
-
+        '''Test the behaviour of an AUTO takeoff, pt1.'''
+        '''
         Conditions:
         - ARSPD_USE=1
         - TKOFF_OPTIONS[0]=0
@@ -4395,6 +4395,7 @@ def TakeoffAuto1(self):
             "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item.
         })
 
+        # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m.
         self.wait_ready_to_arm()
 
         # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m.
@@ -4419,8 +4420,8 @@ def TakeoffAuto1(self):
         self.wait_disarmed(120)
 
     def TakeoffAuto2(self):
-        '''Test the behaviour of an AUTO takeoff, pt2.
-
+        '''Test the behaviour of an AUTO takeoff, pt2.'''
+        '''
         Conditions:
         - ARSPD_USE=0
         - TKOFF_OPTIONS[0]=0
@@ -4442,6 +4443,7 @@ def TakeoffAuto2(self):
             "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item.
         })
 
+        # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m.
         self.wait_ready_to_arm()
 
         # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m.
@@ -4466,8 +4468,8 @@ def TakeoffAuto2(self):
         self.wait_disarmed(120)
 
     def TakeoffAuto3(self):
-        '''Test the behaviour of an AUTO takeoff, pt3.
-
+        '''Test the behaviour of an AUTO takeoff, pt3.'''
+        '''
         Conditions:
         - ARSPD_USE=1
         - TKOFF_OPTIONS[0]=1
@@ -4491,6 +4493,7 @@ def TakeoffAuto3(self):
             "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item.
         })
 
+        # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m.
         self.wait_ready_to_arm()
 
         # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m.
@@ -4510,17 +4513,22 @@ def TakeoffAuto3(self):
         # Ensure that after that the aircraft does not go full throttle anymore.
         test_alt = 50
         self.wait_altitude(test_alt, test_alt+2, relative=True)
-        max_throttle = 1000+10*(self.get_parameter("TKOFF_THR_MAX"))
-        min_throttle = 1000+10*(self.get_parameter("THR_MIN"))
-        self.assert_servo_channel_range(3, min_throttle, max_throttle-1)
+        w = vehicle_test_suite.WaitAndMaintainServoChannelValue(
+            self,
+            3,  # throttle
+            1000+10*self.get_parameter("TKOFF_THR_MAX")-10,
+            comparator=operator.lt,
+            minimum_duration=1,
+        )
+        w.run()
 
         # Wait for landing waypoint.
         self.wait_current_waypoint(11, timeout=1200)
         self.wait_disarmed(120)
 
     def TakeoffAuto4(self):
-        '''Test the behaviour of an AUTO takeoff, pt4.
-
+        '''Test the behaviour of an AUTO takeoff, pt4.'''
+        '''
         Conditions:
         - ARSPD_USE=0
         - TKOFF_OPTIONS[0]=1
@@ -4571,8 +4579,8 @@ def TakeoffAuto4(self):
         self.wait_disarmed(120)
 
     def TakeoffTakeoff1(self):
-        '''Test the behaviour of a takeoff in TAKEOFF mode, pt1.
-
+        '''Test the behaviour of a takeoff in TAKEOFF mode, pt1.'''
+        '''
         Conditions:
         - ARSPD_USE=1
         - TKOFF_OPTIONS[0]=0
@@ -4625,8 +4633,8 @@ def TakeoffTakeoff1(self):
         self.fly_home_land_and_disarm()
 
     def TakeoffTakeoff2(self):
-        '''Test the behaviour of a takeoff in TAKEOFF mode, pt2.
-
+        '''Test the behaviour of a takeoff in TAKEOFF mode, pt2.'''
+        '''
         Conditions:
         - ARSPD_USE=1
         - TKOFF_OPTIONS[0]=1
@@ -4680,8 +4688,8 @@ def TakeoffTakeoff2(self):
         self.fly_home_land_and_disarm()
 
     def TakeoffTakeoff3(self):
-        '''Test the behaviour of a takeoff in TAKEOFF mode, pt3.
-
+        '''Test the behaviour of a takeoff in TAKEOFF mode, pt3.'''
+        '''
         This is the same as case #1, but with disabled airspeed sensor.
 
         Conditions:
@@ -4715,30 +4723,44 @@ def TakeoffTakeoff3(self):
         # Throw the catapult.
         self.set_servo(7, 2000)
 
+        # we expect to maintain this throttle level past the takeoff
+        # altitude through to our takeoff altitude:
+        expected_takeoff_throttle = 1000+10*self.get_parameter("TKOFF_THR_MAX")
+
         # Check whether we're at max throttle below TKOFF_LVL_ALT.
         test_alt = self.get_parameter("TKOFF_LVL_ALT")-10
         self.wait_altitude(test_alt, test_alt+2, relative=True)
-        target_throttle = 1000+10*(self.get_parameter("TKOFF_THR_MAX"))
-        self.assert_servo_channel_range(3, target_throttle-10, target_throttle+10)
+        w = vehicle_test_suite.WaitAndMaintainServoChannelValue(
+            self,
+            3,  # throttle
+            expected_takeoff_throttle,
+            epsilon=10,
+            minimum_duration=1,
+        )
+        w.run()
 
         # Check whether we're still at max throttle past TKOFF_LVL_ALT.
         test_alt = self.get_parameter("TKOFF_LVL_ALT")+10
         self.wait_altitude(test_alt, test_alt+2, relative=True)
-        target_throttle = 1000+10*(self.get_parameter("TKOFF_THR_MAX"))
-        self.assert_servo_channel_range(3, target_throttle-10, target_throttle+10)
+
+        w = vehicle_test_suite.WaitAndMaintainServoChannelValue(
+            self,
+            3,  # throttle
+            expected_takeoff_throttle,
+            epsilon=10,
+            minimum_duration=1,
+        )
+        w.run()
 
         # Wait for the takeoff to complete.
         target_alt = self.get_parameter("TKOFF_ALT")
-        self.wait_altitude(target_alt-5, target_alt, relative=True)
+        self.wait_altitude(target_alt-2.5, target_alt+2.5, relative=True, minimum_duration=10, timeout=30)
 
-        # Wait a bit for the Takeoff altitude to settle.
-        self.delay_sim_time(5)
-
-        self.fly_home_land_and_disarm()
+        self.reboot_sitl(force=True)
 
     def TakeoffTakeoff4(self):
-        '''Test the behaviour of a takeoff in TAKEOFF mode, pt4.
-
+        '''Test the behaviour of a takeoff in TAKEOFF mode, pt4.'''
+        '''
         This is the same as case #3, but with almost stock parameters and without a catapult.
 
         Conditions:
@@ -6104,6 +6126,84 @@ def ForceArm(self):
             )
             self.disarm_vehicle()
 
+    def _MAV_CMD_EXTERNAL_WIND_ESTIMATE(self, command):
+        self.reboot_sitl()
+
+        def cmp_with_variance(a, b, p):
+            return abs(a - b) < p
+
+        def check_eq(speed, direction, ret_dir, timeout=1):
+            command(mavutil.mavlink.MAV_CMD_EXTERNAL_WIND_ESTIMATE, p1=speed, p3=direction)
+
+            tstart = self.get_sim_time()
+            while True:
+                if self.get_sim_time_cached() - tstart > timeout:
+                    raise NotAchievedException(
+                        f"Failed to set wind speed or/and direction: speed != {speed} or direction != {direction}")
+
+                m = self.assert_receive_message("WIND")
+                if cmp_with_variance(m.speed, speed, 0.5) and cmp_with_variance(m.direction, ret_dir, 5):
+                    return True
+
+        check_eq(1, 45, 45)
+        check_eq(2, 90, 90)
+        check_eq(3, 120, 120)
+        check_eq(4, 180, -180)
+        check_eq(5, 240, -120)
+        check_eq(6, 320, -40)
+        check_eq(7, 360, 0)
+
+        command(mavutil.mavlink.MAV_CMD_EXTERNAL_WIND_ESTIMATE, p1=-2, p3=90, want_result=mavutil.mavlink.MAV_RESULT_DENIED)
+        command(mavutil.mavlink.MAV_CMD_EXTERNAL_WIND_ESTIMATE, p1=2, p3=-90, want_result=mavutil.mavlink.MAV_RESULT_DENIED)
+        command(mavutil.mavlink.MAV_CMD_EXTERNAL_WIND_ESTIMATE, p1=-2, p3=-90, want_result=mavutil.mavlink.MAV_RESULT_DENIED)
+        command(mavutil.mavlink.MAV_CMD_EXTERNAL_WIND_ESTIMATE, p1=2, p3=370, want_result=mavutil.mavlink.MAV_RESULT_DENIED)
+
+    def MAV_CMD_EXTERNAL_WIND_ESTIMATE(self):
+        '''test MAV_CMD_EXTERNAL_WIND_ESTIMATE as a mavlink command'''
+        self._MAV_CMD_EXTERNAL_WIND_ESTIMATE(self.run_cmd)
+        self._MAV_CMD_EXTERNAL_WIND_ESTIMATE(self.run_cmd_int)
+
+    def GliderPullup(self):
+        '''test pullup of glider after ALTITUDE_WAIT'''
+        self.start_subtest("test glider pullup")
+
+        self.customise_SITL_commandline(
+            [],
+            model="glider",
+            defaults_filepath="Tools/autotest/default_params/glider.parm",
+            wipe=True)
+
+        self.set_parameters({
+            "PUP_ENABLE": 1,
+            "SERVO6_FUNCTION": 0, # balloon lift
+            "SERVO10_FUNCTION": 156, # lift release
+            "EK3_IMU_MASK": 1, # lane switches just make log harder to read
+        })
+
+        self.set_servo(6, 1000)
+
+        self.load_mission("glider-pullup-mission.txt")
+        self.change_mode("AUTO")
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.context_collect('STATUSTEXT')
+
+        self.progress("Start balloon lift")
+        self.set_servo(6, 2000)
+
+        self.wait_text("Reached altitude", check_context=True, timeout=300)
+        self.wait_text("Start pullup airspeed", check_context=True)
+        self.wait_text("Pullup airspeed", check_context=True)
+        self.wait_text("Pullup pitch", check_context=True)
+        self.wait_text("Pullup level", check_context=True)
+        self.wait_text("Mission complete, changing mode to RTL", check_context=True)
+
+        self.fly_home_land_and_disarm(timeout=400)
+
+    def BadRollChannelDefined(self):
+        '''ensure we don't die with a  bad Roll channel defined'''
+        self.set_parameter("RCMAP_ROLL", 17)
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestPlane, self).tests()
@@ -6236,6 +6336,9 @@ def tests(self):
             self.GPSPreArms,
             self.SetHomeAltChange,
             self.ForceArm,
+            self.MAV_CMD_EXTERNAL_WIND_ESTIMATE,
+            self.GliderPullup,
+            self.BadRollChannelDefined,
         ])
         return ret