diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 5f93cc4729b2b..4bca83cccd874 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/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(); diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index fa4a0949133ff..d2d4f36a877d1 100644 --- a/Tools/autotest/arduplane.py +++ b/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,