From 3201ecd38118443eeb92bafed49699ce105a638c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 30 Jul 2022 12:44:18 +1000
Subject: [PATCH] Plane: added an arming check for VTOL land too short

this is meant to catch bad mission setup, especially for UGCS, which
planes waypoints right on top of the landing point
---
 ArduPlane/AP_Arming.cpp | 24 ++++++++++++++++++++++++
 1 file changed, 24 insertions(+)

diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp
index d456f2c752..e638ec956e 100644
--- a/ArduPlane/AP_Arming.cpp
+++ b/ArduPlane/AP_Arming.cpp
@@ -392,5 +392,29 @@ bool AP_Arming_Plane::mission_checks(bool report)
         ret = false;
         check_failed(ARMING_CHECK_MISSION, report, "DO_LAND_START set and RTL_AUTOLAND disabled");
     }
+#if HAL_QUADPLANE_ENABLED
+    if (plane.quadplane.available()) {
+        const uint16_t num_commands = plane.mission.num_commands();
+        AP_Mission::Mission_Command prev_cmd {};
+        for (uint16_t i=1; i<num_commands; i++) {
+            AP_Mission::Mission_Command cmd;
+            if (!plane.mission.read_cmd_from_storage(i, cmd)) {
+                break;
+            }
+            if ((cmd.id == MAV_CMD_NAV_VTOL_LAND || cmd.id == MAV_CMD_NAV_LAND) &&
+                prev_cmd.id == MAV_CMD_NAV_WAYPOINT) {
+                const float dist = cmd.content.location.get_distance(prev_cmd.content.location);
+                const float tecs_land_speed = plane.TECS_controller.get_land_airspeed();
+                const float landing_speed = is_positive(tecs_land_speed)?tecs_land_speed:plane.aparm.airspeed_cruise_cm*0.01;
+                const float min_dist = 0.75 * plane.quadplane.stopping_distance(sq(landing_speed));
+                if (dist < min_dist) {
+                    ret = false;
+                    check_failed(ARMING_CHECK_MISSION, report, "VTOL land too short, min %.0fm", min_dist);
+                }
+            }
+            prev_cmd = cmd;
+        }
+    }
+#endif
     return ret;
 }