From c15fa7b94375d8cbb2a41382470daa8ca0014cb7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 25 Nov 2024 14:53:26 +1100 Subject: [PATCH] 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 d8e3c11f75..4a8f7aef27 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();