AP_Scripting: quadplane terrain avoidance with can't make that climb

libraries/AP_Scripting/applets/quadplane_terrain_avoid.lua

@@ -35,7 +35,7 @@ altitude to acheive a safe AMSL altitude to avoid the terrain before continuing
 
 SCRIPT_NAME = "OvrhdIntl Terrain Avoid"
 SCRIPT_NAME_SHORT = "TerrAvoid"
-SCRIPT_VERSION = "4.6.0-001"
+SCRIPT_VERSION = "4.7.0-003"
 
 REFRESH_RATE = 0.1  -- in seconds, so 10Hz
 STARTUP_DELAY = 20  -- wait this many seconds for the FC to come up before starting the script
@@ -77,7 +77,7 @@ function bind_add_param2(name, idx, default_value)
     return bind_param(PARAM2_TABLE_PREFIX .. name)
 end
 
--- setup follow mode specific parameters
+-- setup follow mode specific parameters need 2wo tables because there are > 10 parameters
 assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), SCRIPT_NAME_SHORT .. 'could not add param table: ' .. PARAM_TABLE_PREFIX)
 assert(param:add_table(PARAM2_TABLE_KEY, PARAM2_TABLE_PREFIX, 10), SCRIPT_NAME_SHORT .. 'could not add param table: ' .. PARAM2_TABLE_PREFIX)
 
@@ -94,6 +94,7 @@ ZTA_ACT_FN = bind_add_param("ACT_FN", 1, 305)
     // @DisplayName: Terrain Avoidance minimum down distance for Pitching
     // @Description: If the downward distance is less than this value then start Pitching up to gain altitude.
     // @Units: m
+    // @Default: 46
 --]]
 ZTA_PTCH_DWN_MIN = bind_add_param("PTCH_DWN_MIN", 2, 46)
 
@@ -102,6 +103,7 @@ ZTA_PTCH_DWN_MIN = bind_add_param("PTCH_DWN_MIN", 2, 46)
     // @DisplayName: Terrain Avoidance minimum forward distance for Pitching
     // @Description: If the farwardward distance is less than this value then start Pitching up to gain altitude.
     // @Units: m
+    // @Default: 80
 --]]
 ZTA_PTCH_FWD_MIN = bind_add_param("PTCH_FWD_MIN", 3, 80)
 
@@ -110,6 +112,7 @@ ZTA_PTCH_FWD_MIN = bind_add_param("PTCH_FWD_MIN", 3, 80)
     // @DisplayName: Terrain Avoidance minimum down distance for Quading
     // @Description: If the downward distance is less than this value then start Quading up to gain altitude.
     // @Units: m
+    // @Default: 46
 --]]
 ZTA_QUAD_DWN_MIN = bind_add_param("QUAD_DWN_MIN", 4, 35)
 
@@ -118,6 +121,7 @@ ZTA_QUAD_DWN_MIN = bind_add_param("QUAD_DWN_MIN", 4, 35)
     // @DisplayName: Terrain Avoidance minimum forward distance for Quading
     // @Description: If the farwardward distance is less than this value then start Quading up to gain altitude.
     // @Units: m
+    // @Default: 20
 --]]
 ZTA_QUAD_FWD_MIN = bind_add_param("QUAD_FWD_MIN", 5, 20)
 
@@ -126,6 +130,7 @@ ZTA_QUAD_FWD_MIN = bind_add_param("QUAD_FWD_MIN", 5, 20)
     // @DisplayName: Terrain Avoidance minimum ground speed for Pitching
     // @Description: Minimum Groundspeed (not airspeed) to be flying for Pitching to be used.
     // @Units: m/s
+    // @Default: 17
 --]]
 ZTA_PTCH_GSP_MIN = bind_add_param("PTCH_GSP_MIN", 6, 17)
 
@@ -134,6 +139,7 @@ ZTA_PTCH_GSP_MIN = bind_add_param("PTCH_GSP_MIN", 6, 17)
     // @DisplayName: Terrain Avoidance timeout Pitching
     // @Description: Minimum down or forward distance must be triggered for more than this many seconds to start Pitching
     // @Units: s
+    // @Default: 2
 --]]
 ZTA_PTCH_TIMEOUT = bind_add_param("PTCH_TIMEOUT", 7, 2)
 
@@ -142,6 +148,7 @@ ZTA_PTCH_TIMEOUT = bind_add_param("PTCH_TIMEOUT", 7, 2)
     // @DisplayName: Terrain Avoidance safe distance around home
     // @Description: Terrain avoidance will not be applied if the vehicle is less than this distance from home
     // @Units: m
+    // @Default: 20
 --]]
 ZTA_HOME_DIST = bind_add_param("HOME_DIST", 8, 20)
 
@@ -168,6 +175,7 @@ ZTA_SIM_FWD_FN = bind_add_param("SIM_FWD_FN", 10, 307)
     // @Description: This is a limit on how high the terrain avoidane will take the vehicle. It acts a failsafe to prevent vertical flyaways.
     // @Range: 20 1000
     // @Units: m
+    // @Default: 100
 --]]
 ZTA_ALT_MAX = bind_add_param("ALT_MAX", 9, 100)
 
@@ -177,6 +185,7 @@ ZTA_ALT_MAX = bind_add_param("ALT_MAX", 9, 100)
     // @Description: This is a limit on how fast in groundspeeed terrain avoidance will take the vehicle. This is to allow for reliable sensor readings.
     // @Range: 10 40
     // @Units: m/s
+    // @Default: -1 (disabed)
 --]]
 ZTB_GSP_MAX = bind_add_param2("GSP_MAX", 1, -1)
 
@@ -186,9 +195,28 @@ ZTB_GSP_MAX = bind_add_param2("GSP_MAX", 1, -1)
     // @Description: This is the limit for triggering airbrake to slow groundspeed as a difference between the airspeed and groundspeed
     // @Range: -1 -10
     // @Units: m/s
+    // @Default: 0 (disabed)
 --]]
 ZTB_GSP_AIRBRAKE = bind_add_param2("GSP_AIRBRAKE", 2, 0)
 
+--[[
+    // @Param: ZTB_CMTC_ALT
+    // @DisplayName: CMTC Altitude 
+    // @Description: The minimum altitude above terrain to maintain when following an AUTO mission or RTL. -1 to disable CMTC.
+    // @Units: m
+    // @Default: 0 (use ZTA_PTCH_DWN_MIN value)
+--]]
+ZTB_CMTC_ALT = bind_add_param2("CMTC_ALT", 3, 0)
+
+--[[
+    // @Param: ZTB_CMTC_ENABLE
+    // @DisplayName: CMTC Enable
+    // @Description: Whether to enable Can't Make That Climb while running Terrain Avoidance
+    // @Range: 0 1
+    // @Default: 0 
+--]]
+ZTB_CMTC_ENABLE = bind_add_param2("CMTC_ENABLE", 4, 0)
+
 local pitch_groundspeed_min = ZTA_PTCH_GSP_MIN:get()
 local pitch_down_min = ZTA_PTCH_DWN_MIN:get()
 local pitch_forward_min = ZTA_PTCH_FWD_MIN:get()
@@ -203,6 +231,12 @@ local altitude_max = ZTA_ALT_MAX:get()
 local groundspeed_max = ZTB_GSP_MAX:get() or -1
 local groundspeed_airbrake_limit = ZTB_GSP_AIRBRAKE:get() or 0
 
+local cmtc_alt_m = ZTB_CMTC_ALT:get()
+if cmtc_alt_m == 0 then
+    cmtc_alt_m = pitch_down_min
+end
+local cmtc_enable = ZTB_CMTC_ENABLE:get()
+
 MIN_ALT_MAX = 20
 
 Q_ENABLE = bind_param('Q_ENABLE')
@@ -339,7 +373,7 @@ local function slowdown_airbrake_end()
     enable_wvane()
     gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": airbraking stopping")
     if vehicle_mode ~= FLIGHT_MODE.QLOITER then
-        -- user or a failesafe has exitied QLoiter, so we don't want to mess with that
+        -- user or a failesafe has exited QLoiter, so we don't want to mess with that
         return
     end
     reset_avoid_mode()
@@ -379,6 +413,7 @@ local function slow_down(groundspeed)
 end
 
 local function set_altitude_target(new_altitude)
+    current_location_target = vehicle:get_target_location()
     if current_location_target ~= nil then
         new_location_target = current_location_target:copy()
         if new_location_target ~= nil and new_altitude ~= nil then
@@ -457,6 +492,9 @@ end
 location_tracker = LocationTracker()
 
 local function start_cmtc(target_alt_amsl)
+    if not cmtc_enable then
+        return
+    end
     if cmtc_active then
         gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": CMTC to ALREADY ACTIVE: " .. cmtc_target_alt_amsl)
         return
@@ -737,7 +775,7 @@ function quad_obstacle_detected()
     return false
 end
 
--- this method checks the distance down and forward. There are no sanity checks (yet)
+-- this method checks the distance down and forward. 
 -- and this uses RC8 to simulate forward rangefinder and RC5 to simulate downward
 local function populate_rangefinder_values()
     -- Get the new values of the range finders every update cycle
@@ -764,7 +802,7 @@ local function populate_rangefinder_values()
         terrain_max_exceeded = (altitude_max > MIN_ALT_MAX and terrain_altitude > altitude_max)
     end
 
-    if rangefinder_down_value == nil or rangefinder_down_value <= 0 or 
+    if rangefinder_down_value == nil or rangefinder_down_value <= 0 or
         rangefinder_down_value > MAX_RANGEFINDER_VALUE then
         rangefinder_down_value = terrain_altitude or 0
     end
@@ -785,7 +823,7 @@ local function wrap_360(angle)
 end
 
 --[[
-c++_code used as reference
+c++_code from AP_Terrain.cpp used as reference
 // check for terrain at grid spacing intervals
 while (distance > 0) {
     gcs().send_text(MAV_SEVERITY_INFO, "lookahead distance %.1f", distance);
@@ -837,8 +875,8 @@ local function terrain_lookahead(start_location, search_bearing, search_distance
     return highest_location
 end
 
--- returns required pitch to avoid hitting something between now and the next waypoint or other destination such as
--- home locaiton for RTL
+-- returns required pitch to avoid hitting something between here and the next waypoint or other destination such as
+-- home location for RTL
 terrain_approaching = function(clearance)
     if current_location == nil then
         gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": current_location NIL") )
@@ -865,7 +903,6 @@ terrain_approaching = function(clearance)
 
     -- need to know how far ahead the highest location is and how much higher than the current 
     -- altitude to calculate a minimum required glide slope (which TECS already does)
-    --local highest_altitude_amsl = highest_location:alt() * 0.01
     highest_location:change_alt_frame(mavlink.ALT_FRAME.ABSOLUTE)
     local altitude_difference =  (highest_location:alt() * 0.01) + clearance - (current_location_amsl:alt() * 0.01)
     if altitude_difference > 0 then
@@ -972,7 +1009,7 @@ local function check_activation_switch()
     if (switch_state ~= last_switch_state) then
         if switch_state == 0 then -- switch Low to activate - so defaults to on
             activate()
-        elseif switch_state == 2 then -- switch Jight to turn off
+        elseif switch_state == 2 then -- switch High to turn off
             deactivate()
         end
         -- Don't know what to do with the 3rd switch position right now.
@@ -1053,14 +1090,19 @@ local function update()
         groundspeed_airbrake_limit = ZTB_GSP_AIRBRAKE:get() or 0
         airspeed_min = AIRSPEED_MIN:get() or 5
         airspeed_max = AIRSPEED_MAX:get() or 30
+
+        cmtc_alt_m = ZTB_CMTC_ALT:get()
+        if cmtc_alt_m == 0 then
+            cmtc_alt_m = pitch_down_min
+        end
+        cmtc_enable = ZTB_CMTC_ENABLE:get()
     end
     check_activation_switch()
     if not terravoid_active() then
         if quading_active then
             stop_quading()
         end
         if pitching_active then
-            gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": Stop Pitching done update")
             stop_pitching()
         end
         pitching_active = false
@@ -1075,12 +1117,12 @@ local function update()
     if not quading_active and not check_quading() and not cmtc_active then
         -- lets check if our current flight path is likely to hit terrain 
         -- sometime soon, and if so we need to avoid it.
-        local pitch_required_deg, alt_required_amsl, terrain_diff = terrain_approaching(pitch_down_min)
+        local pitch_required_deg, alt_required_amsl, terrain_diff = terrain_approaching(cmtc_alt_m)
         if pitch_required_deg > (ptch_lim_max_deg * 0.5) then
             gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. string.format(": Can't Make that climb", terrain_diff) )
             gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. string.format(": CMTC pitch required %.0f deg", pitch_required_deg) )
-            -- need to fly OVER the highest point - with pitch_down_min clearance * 2
-            start_cmtc(alt_required_amsl + pitch_down_min * 2.0)
+            -- need to fly OVER the highest point - with ZTB_CMTC_ALT clearance
+            start_cmtc(alt_required_amsl + cmtc_alt_m)
         else
             -- otherwise - lets see if we are close enough to need to start pitching
             check_pitching()

libraries/AP_Scripting/applets/quadplane_terrain_avoid.md

@@ -80,6 +80,17 @@ isn't working then if this is set to 1, the script will attempt to use QHOVER
 to reduce airspeed. This doesn't work very well, so test it for your use case.
 It defaults to off.
 
+# ZTB_CMTC_ENABLE
+
+Enable the Can't Make That Climb (CMTC) feature, which will circle to gain altitude if 
+the required pitch up to the next waypoint exceeds PTCH_LIM_MAX_DEG / 2
+
+# ZTB_CMTC_ALT
+
+If CMTC is enabled, uses this altidude as the clearance required above terrain altitude to 
+use for CMTC calculation. If the plane can't make this number of meters clearance above the
+terrain between the current location and the next waypoint then CMTC will be engaged.
+
 # Operation
 
 Good TECS tuning of your aircraft is essential. The script relies