From 150afa4d7c2098580d03e288af78ffb219910c36 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 25 Nov 2024 14:53:26 +1100 Subject: [PATCH 1/2] Plane: fixed takeoff direction with no yaw source in TAKEOFF mode with either very poor yaw source or no yaw source we need to use ground vector and wait for sufficient ground speed --- ArduPlane/mode_takeoff.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 5f93cc4729b2b..dcbba99d94c81 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -83,7 +83,10 @@ 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 (altitude >= alt) { @@ -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(); From 6b73c7fde9e87e3d0f30e9ea36159d1b9d5e0e3c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 25 Nov 2024 15:39:37 +1100 Subject: [PATCH 2/2] autotest: added non-compass takeoff test --- ArduPlane/mode_takeoff.cpp | 2 +- Tools/autotest/arduplane.py | 36 ++++++++++++++++++++++++++++++++++++ 2 files changed, 37 insertions(+), 1 deletion(-) diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index dcbba99d94c81..4bca83cccd874 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -88,7 +88,7 @@ void ModeTakeoff::update() 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; 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,