Merge branch 'master' into pr-add-vs-code-launch-auto-generation

.gitignore

@@ -171,7 +171,3 @@ ENV/
 env.bak/
 venv.bak/
 autotest_result_*_junit.xml
-
-# Ignore ESP-IDF SDK defines
-/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig
-/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig

ArduPlane/mode_takeoff.cpp

@@ -83,9 +83,12 @@ void ModeTakeoff::update()
     if (!takeoff_mode_setup) {
         plane.auto_state.takeoff_altitude_rel_cm = alt * 100;
         const uint16_t altitude = plane.relative_ground_altitude(false,true);
-        const float direction = degrees(ahrs.get_yaw());
+        const Vector2f &groundspeed2d = ahrs.groundspeed_vector();
+        const float direction = wrap_360(degrees(groundspeed2d.angle()));
+        const float groundspeed = groundspeed2d.length();
+
         // see if we will skip takeoff as already flying
-        if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && ahrs.groundspeed() > 3) {
+        if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && groundspeed > 3) {
             if (altitude >= alt) {
                 gcs().send_text(MAV_SEVERITY_INFO, "Above TKOFF alt - loitering");
                 plane.next_WP_loc = plane.current_loc;
@@ -115,7 +118,15 @@ void ModeTakeoff::update()
 
             plane.set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF);
 
-            if (!plane.throttle_suppressed) {
+            /*
+              don't lock in the takeoff loiter location until we reach
+              a ground speed of AIRSPEED_MIN*0.3 to cope with aircraft
+              with no compass or poor compass. If flying in a very
+              strong headwind then the is_flying() check above will
+              eventually trigger
+             */
+            if (!plane.throttle_suppressed &&
+                groundspeed > plane.aparm.airspeed_min*0.3) {
                 gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg",
                                 alt, dist, direction);
                 plane.takeoff_state.start_time_ms = millis();

Tools/AP_Bootloader/board_types.txt

@@ -331,6 +331,7 @@ AP_HW_MountainEagleH743              1444
 
 AP_HW_StellarF4                      1500
 AP_HW_GEPRCF745BTHD                  1501
+AP_HW_GEPRC_TAKER_H743               1502
 
 AP_HW_HGLRCF405V4                    1524
 

Tools/AP_Periph/imu.cpp

@@ -11,8 +11,15 @@ extern const AP_HAL::HAL &hal;
 void AP_Periph_FW::can_imu_update(void)
 {
     while (true) {
-        // sleep for a bit to avoid flooding the CPU
-        hal.scheduler->delay_microseconds(100);
+        // we need to delay by a ms value as hal->schedule->delay_microseconds_boost
+        // used in wait_for_sample() takes uint16_t
+        const uint32_t delay_ms = 1000U / g.imu_sample_rate;
+        hal.scheduler->delay(delay_ms);
+
+        if (delay_ms == 0) {
+            // sleep for a bit to avoid flooding the CPU
+            hal.scheduler->delay_microseconds(100);
+        }
 
         imu.update();
 
@@ -38,6 +45,11 @@ void AP_Periph_FW::can_imu_update(void)
         pkt.accelerometer_latest[1] = tmp.y;
         pkt.accelerometer_latest[2] = tmp.z;
 
+        tmp = imu.get_gyro();
+        pkt.rate_gyro_latest[0] = tmp.x;
+        pkt.rate_gyro_latest[1] = tmp.y;
+        pkt.rate_gyro_latest[2] = tmp.z;
+
         uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_RAWIMU_MAX_SIZE];
         uint16_t total_size = uavcan_equipment_ahrs_RawIMU_encode(&pkt, buffer, !canfdout());
         canard_broadcast(UAVCAN_EQUIPMENT_AHRS_RAWIMU_SIGNATURE,

Tools/ardupilotwaf/boards.py

@@ -949,6 +949,7 @@ def configure_env(self, cfg, env):
             HAL_PERIPH_ENABLE_AIRSPEED = 1,
             HAL_PERIPH_ENABLE_MAG = 1,
             HAL_PERIPH_ENABLE_BARO = 1,
+            HAL_PERIPH_ENABLE_IMU = 1,
             HAL_PERIPH_ENABLE_RANGEFINDER = 1,
             HAL_PERIPH_ENABLE_BATTERY = 1,
             HAL_PERIPH_ENABLE_EFI = 1,
@@ -963,6 +964,7 @@ def configure_env(self, cfg, env):
             HAL_WITH_ESC_TELEM = 1,
             AP_EXTENDED_ESC_TELEM_ENABLED = 1,
             AP_TERRAIN_AVAILABLE = 1,
+            HAL_GYROFFT_ENABLED = 0,
         )
 
 class sitl_periph_gps(sitl_periph):

Tools/ardupilotwaf/esp32.py

@@ -57,6 +57,24 @@ def bldpath(path):
 
     #env.append_value('GIT_SUBMODULES', 'esp_idf')
 
+# delete the output sdkconfig file when the input defaults changes. we take the
+# stamp as the output so we can compute the path to the sdkconfig, yet it
+# doesn't have to exist when we're done.
+class clean_sdkconfig(Task.Task):
+    def keyword(self):
+        return "delete sdkconfig generated from"
+
+    def run(self):
+        prefix = ".clean-stamp-"
+        for out in self.outputs:
+            if not out.name.startswith(prefix):
+                raise ValueError("not a stamp file: "+out)
+            dest = out.parent.abspath()+"/"+out.name[len(prefix):]
+            if os.path.exists(dest):
+                os.unlink(dest)
+
+            # waf needs the output to exist after the task, so touch it
+            open(out.abspath(), "w").close()
 
 def pre_build(self):
     """Configure esp-idf as lib target"""
@@ -74,8 +92,21 @@ def pre_build(self):
             )
 
     esp_idf_showinc = esp_idf.build('showinc', target='esp-idf_build/includes.list')
+
+    # task to delete the sdkconfig (thereby causing it to be regenerated) when
+    # the .defaults changes. it uses a stamp to find the sdkconfig. changing
+    # the sdkconfig WILL NOT cause it to be deleted as it's not an input. this
+    # is by design so the user can tweak it for testing purposes.
+    clean_sdkconfig_task = esp_idf_showinc.create_task("clean_sdkconfig",
+        src=self.srcnode.find_or_declare(self.env.AP_HAL_ESP32+"/sdkconfig.defaults"),
+        tgt=self.bldnode.find_or_declare("esp-idf_build/.clean-stamp-sdkconfig"))
+
     esp_idf_showinc.post()
 
+    # ensure the sdkconfig will be deleted before the cmake configure occurs
+    # that regenerates it
+    esp_idf_showinc.cmake_config_task.set_run_after(clean_sdkconfig_task)
+
     from waflib import Task
     class load_generated_includes(Task.Task):
         """After includes.list generated include it in env"""

Tools/autotest/arducopter.py

@@ -8304,6 +8304,57 @@ def WatchAlts(self):
 
         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 @@ def tests2b(self):  # this block currently around 9.5mins here
             self.MAV_CMD_MISSION_START_p1_p2,
             self.ScriptingAHRSSource,
             self.CommonOrigin,
+            self.TestTetherStuck,
         ])
         return ret
 

Tools/autotest/arduplane.py

@@ -4805,6 +4805,41 @@ def TakeoffTakeoff4(self):
 
         self.fly_home_land_and_disarm()
 
+    def TakeoffTakeoff5(self):
+        '''Test the behaviour of a takeoff with no compass'''
+        self.set_parameters({
+            "COMPASS_USE": 0,
+            "COMPASS_USE2": 0,
+            "COMPASS_USE3": 0,
+        })
+        import copy
+        start_loc = copy.copy(SITL_START_LOCATION)
+        start_loc.heading = 175
+        self.customise_SITL_commandline(["--home=%.9f,%.9f,%.2f,%.1f" % (
+            start_loc.lat, start_loc.lng, start_loc.alt, start_loc.heading)])
+        self.reboot_sitl()
+        self.change_mode("TAKEOFF")
+
+        # waiting for the EKF to be happy won't work
+        self.delay_sim_time(20)
+        self.arm_vehicle()
+
+        target_alt = self.get_parameter("TKOFF_ALT")
+        self.wait_altitude(target_alt-5, target_alt, relative=True)
+
+        # Wait a bit for the Takeoff altitude to settle.
+        self.delay_sim_time(5)
+
+        bearing_margin = 35
+        loc = self.mav.location()
+        bearing_from_home = self.get_bearing(start_loc, loc)
+        if bearing_from_home < 0:
+            bearing_from_home += 360
+        if abs(bearing_from_home - start_loc.heading) > bearing_margin:
+            raise NotAchievedException(f"Did not takeoff in the right direction {bearing_from_home}")
+
+        self.fly_home_land_and_disarm()
+
     def TakeoffGround(self):
         '''Test a rolling TAKEOFF.'''
 
@@ -5326,6 +5361,40 @@ def AerobaticsScripting(self):
             else:
                 raise NotAchievedException("Missing trick %s" % t)
 
+    def UniversalAutoLandScript(self):
+        '''Test UniversalAutoLandScript'''
+        applet_script = "UniversalAutoLand.lua"
+        self.customise_SITL_commandline(["--home", "-35.362938,149.165085,585,173"])
+
+        self.install_applet_script_context(applet_script)
+        self.context_collect('STATUSTEXT')
+        self.set_parameters({
+            "SCR_ENABLE" : 1,
+            "SCR_VM_I_COUNT" : 1000000,
+            "RTL_AUTOLAND" : 2
+            })
+        self.reboot_sitl()
+        self.wait_text("Loaded UniversalAutoLand.lua", check_context=True)
+        self.set_parameters({
+             "AUTOLAND_ENABLE" : 1,
+             "AUTOLAND_WP_ALT" : 55,
+             "AUTOLAND_WP_DIST" : 400
+            })
+        self.scripting_restart()
+        self.wait_text("Scripting: restarted", check_context=True)
+
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.change_mode("AUTO")
+        self.wait_text("Captured initial takeoff direction", check_context=True)
+
+        self.wait_disarmed(120)
+        self.progress("Check the landed heading matches takeoff")
+        self.wait_heading(173, accuracy=5, timeout=1)
+        loc = mavutil.location(-35.362938, 149.165085, 585, 173)
+        if self.get_distance(loc, self.mav.location()) > 35:
+            raise NotAchievedException("Did not land close to home")
+
     def SDCardWPTest(self):
         '''test BRD_SD_MISSION support'''
         spiral_script = "mission_spiral.lua"
@@ -6457,6 +6526,7 @@ def tests1a(self):
             self.Soaring,
             self.Terrain,
             self.TerrainMission,
+            self.UniversalAutoLandScript,
         ])
         return ret
 
@@ -6490,6 +6560,7 @@ def tests1b(self):
             self.TakeoffTakeoff2,
             self.TakeoffTakeoff3,
             self.TakeoffTakeoff4,
+            self.TakeoffTakeoff5,
             self.TakeoffGround,
             self.TakeoffIdleThrottle,
             self.TakeoffBadLevelOff,