From 06d305d1ae99ec66786863b9cdc7416508a5b634 Mon Sep 17 00:00:00 2001 From: timtuxworth Date: Thu, 12 Dec 2024 11:11:14 -0700 Subject: [PATCH] AP_Scripting: quadplane terrain avoidance with can't make that climb --- .../applets/quadplane_terrain_avoid.lua | 70 +++++++++++++++---- .../applets/quadplane_terrain_avoid.md | 11 +++ 2 files changed, 67 insertions(+), 14 deletions(-) diff --git a/libraries/AP_Scripting/applets/quadplane_terrain_avoid.lua b/libraries/AP_Scripting/applets/quadplane_terrain_avoid.lua index 7c577b8819b4f..eeeea6b92cb96 100644 --- a/libraries/AP_Scripting/applets/quadplane_terrain_avoid.lua +++ b/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,6 +1090,12 @@ 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 @@ -1060,7 +1103,6 @@ local function update() 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() diff --git a/libraries/AP_Scripting/applets/quadplane_terrain_avoid.md b/libraries/AP_Scripting/applets/quadplane_terrain_avoid.md index 51e6d808b4874..b7a771f5c14b8 100644 --- a/libraries/AP_Scripting/applets/quadplane_terrain_avoid.md +++ b/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