Plane: fixed takeoff direction with no yaw source

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/autotest/arduplane.py

@@ -8,6 +8,7 @@
 import math
 import os
 import signal
+import copy
 
 from pymavlink import quaternion
 from pymavlink import mavutil
@@ -4804,6 +4805,40 @@ 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,
+        })
+        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.'''
 
@@ -6416,6 +6451,7 @@ def tests1b(self):
             self.TakeoffTakeoff2,
             self.TakeoffTakeoff3,
             self.TakeoffTakeoff4,
+            self.TakeoffTakeoff5,
             self.TakeoffGround,
             self.TakeoffIdleThrottle,
             self.ForcedDCM,