From 0a63c6757a4c1cfd330a499979bc99862b7b0ca7 Mon Sep 17 00:00:00 2001 From: timtuxworth Date: Thu, 29 Aug 2024 17:54:28 -0600 Subject: [PATCH] AP_Scripting: quadplane terrain avoidance with can't make that climb --- .../applets/quadplane_terrain_avoid.lua | 1181 +++++++++++++++++ .../applets/quadplane_terrain_avoid.md | 106 ++ libraries/AP_Scripting/docs/docs.lua | 1 - .../AP_Scripting/modules/mavlink_cmdint.lua | 145 ++ libraries/AP_Scripting/modules/speedpi.lua | 155 +++ 5 files changed, 1587 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_Scripting/applets/quadplane_terrain_avoid.lua create mode 100644 libraries/AP_Scripting/applets/quadplane_terrain_avoid.md create mode 100644 libraries/AP_Scripting/modules/mavlink_cmdint.lua create mode 100644 libraries/AP_Scripting/modules/speedpi.lua diff --git a/libraries/AP_Scripting/applets/quadplane_terrain_avoid.lua b/libraries/AP_Scripting/applets/quadplane_terrain_avoid.lua new file mode 100644 index 0000000000000..3331bd02c7b36 --- /dev/null +++ b/libraries/AP_Scripting/applets/quadplane_terrain_avoid.lua @@ -0,0 +1,1181 @@ +--[[ + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + Terrain Avoidance in QuadPlane. + + This code will detect if a quadplane following an Auto mission is likely to hit elevated terrain, such as + a small hill, cliff edge, high trees or other obstacles that might not show up in the OOTB STRM terrain model. + The code will attempt to avoid the impact by:- + - Pitching up if the plane can safely fly over the obstacle + - Otherwise switching to QuadPlane Qloiter mode (Quading) and gaining altitude using VTOL motors + This code requires long range rangefinders such as the LightWare long range lidars that can measure + distances up to 90-95 meters away. + The terrain avoidance will be on by default but will not function at "home" or within ZTA_HOME_DIST meters + of home. The scripting function ZTA_ACT_FN can be used to disable terrain folling at any time + Terrain following will operate in modes Auto, Gukded, RTL and QRTL. + +The "Can't make that climb" (CMTC) feature will prevent ArduPlane from flying into terrain it does know about +by calculating the required pitch to avoid terrain between the current location and the next waypoint including +all points in between. If the pitch required is > PTCH_TRIM_MAX_DEG / 2 then the code will perform a loiter to +altitude to acheive a safe AMSL altitude to avoid the terrain before continuing the mission. + +--]] + +SCRIPT_NAME = "OvrhdIntl Terrain Avoid" +SCRIPT_NAME_SHORT = "TerrAvoid" +SCRIPT_VERSION = "4.7.0-004" + +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 + +FLIGHT_MODE = {AUTO=10, RTL=11, LOITER=12, GUIDED=15, QHOVER=18, QLOITER=19, QRTL=21} + +MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} +MAV_FRAME = { GLOBAL = 0, GLOBAL_RELATIVE_ALT = 3} +MAV_CMD_INT = { DO_SET_MODE = 176, DO_CHANGE_SPEED = 178, DO_REPOSITION = 192, + GUIDED_CHANGE_SPEED = 43000, GUIDED_CHANGE_ALTITUDE = 43001, GUIDED_CHANGE_HEADING = 43002 } +MAV_SPEED_TYPE = { AIRSPEED = 0, GROUNDSPEED = 1, CLIMB_SPEED = 2, DESCENT_SPEED = 3 } +MAV_HEADING_TYPE = { COG = 0, HEADING = 1} -- COG = Course over Ground, i.e. where you want to go, HEADING = which way the vehicle points + +RANGEFINDER_ORIENT = {DOWNWARD = 25, FORWARD = 0} +RANGEFINDER_STATUS = {NOTCONNECTED = 0, NODATA = 1, OUTOFRANGELOW = 2, OUTOFRANGEHIGH = 3, GOOD = 4} + +local rangefinder_down_value = 0.0 +local rangefinder_forward_value = 0.0 +MAX_RANGEFINDER_VALUE = 90 + +local PARAM_TABLE_KEY = 99 +local PARAM_TABLE_PREFIX = "ZTA_" +local PARAM2_TABLE_KEY = 100 +local PARAM2_TABLE_PREFIX = "ZTB_" + +-- bind a parameter to a variable +function bind_param(name) + return Parameter(name) +end + +-- add a parameter and bind it to a variable +function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), SCRIPT_NAME_SHORT .. string.format('could not add param %s', name)) + return bind_param(PARAM_TABLE_PREFIX .. name) +end +-- add a parameter and bind it to a variable +function bind_add_param2(name, idx, default_value) + assert(param:add_param(PARAM2_TABLE_KEY, idx, name, default_value), SCRIPT_NAME_SHORT .. string.format('could not add param %s', name)) + return bind_param(PARAM2_TABLE_PREFIX .. name) +end + +-- 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) + +--[[ + // @Param: ZTA_ACT_FN + // @DisplayName: Terrain Avoidance Activation Function + // @Description: Setting an RC channel's _OPTION to this value will use it for Terrain Avoidance enable/disable + // @Range: 300 307 +--]] +ZTA_ACT_FN = bind_add_param("ACT_FN", 1, 305) + +--[[ + // @Param: ZTA_PTCH_DWN_MIN + // @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 +--]] +ZTA_PTCH_DWN_MIN = bind_add_param("PTCH_DWN_MIN", 2, 46) + +--[[ + // @Param: ZTA_PTCH_FWD_MIN + // @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 +--]] +ZTA_PTCH_FWD_MIN = bind_add_param("PTCH_FWD_MIN", 3, 80) + +--[[ + // @Param: ZTA_QUAD_DWN_MIN + // @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 +--]] +ZTA_QUAD_DWN_MIN = bind_add_param("QUAD_DWN_MIN", 4, 35) + +--[[ + // @Param: ZTA_QUAD_FWD_MIN + // @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 +--]] +ZTA_QUAD_FWD_MIN = bind_add_param("QUAD_FWD_MIN", 5, 20) + +--[[ + // @Param: ZTA_gitlog_MIN + // @DisplayName: Terrain Avoidance minimum ground speed for Pitching + // @Description: Minimum Groundspeed (not airspeed) to be flying for Pitching to be used. + // @Units: m/s +--]] +ZTA_PTCH_GSP_MIN = bind_add_param("PTCH_GSP_MIN", 6, 17) + +--[[ + // @Param: ZTA_PTCH_TIMEOUT + // @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 +--]] +ZTA_PTCH_TIMEOUT = bind_add_param("PTCH_TIMEOUT", 7, 2) + +--[[ + // @Param: ZTA_HOME_DIST + // @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 +--]] +ZTA_HOME_DIST = bind_add_param("HOME_DIST", 8, 20) + +--[[ + // @Param: ZTA_SIM_DWN_FN + // @DisplayName: Terrain Avoidance Sim Down Function + // @Description: Setting an RC channel's _OPTION to this value will use it to generate a simulated "down" rangefinder value based on the PWM value of the channel + // @Range: 300 307 +-- +ZTA_SIM_DWN_FN = bind_add_param("SIM_DWN_FN", 9, 306) + +--[[ + // @Param: ZTA_SIM_FWD_FN + // @DisplayName: Terrain Avoidance Sim Forward Function + // @Description: Setting an RC channel's _OPTION to this value will use it to generate a simulated "forward" rangefinder value based on the PWM value of the channel + // @Range: 300 307 +-- +ZTA_SIM_FWD_FN = bind_add_param("SIM_FWD_FN", 10, 307) +--]] + +--[[ + // @Param: ZTA_ALT_MAX + // @DisplayName: Terrain Avoidance Altitude ceiling for pitching/quading + // @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 +--]] +ZTA_ALT_MAX = bind_add_param("ALT_MAX", 9, 100) + +--[[ + // @Param: ZTB_GSP_MAX + // @DisplayName: Maximum Groundspeed + // @Description: This is a limit on how fast in groundspeeed terrain avoidance will take the vehicle. This is to allow for reliable sensor readings. -1 for disabled. + // @Range: 10 40 + // @Units: m/s +--]] +ZTB_GSP_MAX = bind_add_param2("GSP_MAX", 1, -1) + +--[[ + // @Param: ZTB_GSP_AIRBRAKE + // @DisplayName: Groudspeed Airbrake limt + // @Description: This is the limit for triggering airbrake to slow groundspeed as a difference between the airspeed and groundspeed. -1 for disabled. + // @Range: -1 -10 + // @Units: m/s +--]] +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. If zero(0) use ZTA_PTCH_DOW_MIN. + // @Units: m +--]] +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 +--]] +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() +local pitch_timeout = ZTA_PTCH_TIMEOUT:get() +local home_distance_max = ZTA_HOME_DIST:get() + +local quad_down_min = ZTA_QUAD_DWN_MIN:get() +local quad_forward_min = ZTA_QUAD_FWD_MIN:get() + +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') +Q_WVANE_ENABLE = bind_param('Q_WVANE_ENABLE') +THR_MAX = bind_param('THR_MAX') +AIRSPEED_MIN = bind_param("AIRSPEED_MIN") +AIRSPEED_MAX = bind_param("AIRSPEED_MAX") +WP_LOITER_RAD = bind_param("WP_LOITER_RAD") +TERRAIN_ENABLE = bind_param("TERRAIN_ENABLE") +TERRAIN_SPACING = bind_param("TERRAIN_SPACING") +PTCH_LIM_MAX_DEG = bind_param("PTCH_LIM_MAX_DEG") +TECS_CLMB_MAX = bind_param("TECS_CLMB_MAX") +local airspeed_min = AIRSPEED_MIN:get() or 10 +local airspeed_max = AIRSPEED_MAX:get() or 30 +local wp_loiter_rad = WP_LOITER_RAD:get() or 150 +local q_enable = Q_ENABLE:get() or 0 +local terrain_enable = TERRAIN_ENABLE:get() or 0 +local terrain_spacing = TERRAIN_SPACING:get() or 100 +local ptch_lim_max_deg = PTCH_LIM_MAX_DEG:get() or 25 +local tecs_climb_max = TECS_CLMB_MAX:get() or 5.0 + +RCMAP_THROTTLE = bind_param('RCMAP_THROTTLE') +THROTTLE_CHANNEL = rc:get_channel(RCMAP_THROTTLE:get()) -- The RC channel used for throttle + +PITCH_TOLERANCE = 1.1 + +local vehicle_mode = vehicle:get_mode() + +local current_location = ahrs:get_location() +local current_bearing = 0.0 +local previous_location +local current_altitude = 0.0 +local current_location_target +local new_location_target = Location() +local terrain_altitude = terrain:height_above_terrain(true) +local terrain_max_exceeded = false +local groundspeed_vector = ahrs:groundspeed_vector() +local ground_speed = groundspeed_vector:length() + +local now = millis():tofloat() * 0.001 +local pitch_last_good_timestamp = now +local pitch_last_bad_timestamp = now +local pitch_bad_timer = -10 + +local pre_pitch_mode = vehicle_mode +local pre_pitch_target_altitude= 0.0 +local pre_pitch_target_location + +local quading_active = false +local pitching_active = false +local slowdown_quading = false +local cmtc_active = false + +local q_wvane_enable_save = Q_WVANE_ENABLE:get() +local avoid_enter_mode = -1 + +local cmtc_target_alt_amsl = -1 + +-- function forward declarations +local start_quading -- foward declaration of start_quading/stop_quading defined below +local stop_quading +local avoid_terrain +local terrain_approaching +local pitch_obstacle_detected +local pitch_obstacle_down +local pitch_obstacle_forward + +local speedpi = require("speedpi") +local speed_PI = speedpi.speed_controller(0.1, 0.1, 2.5, airspeed_min, airspeed_max) + +local mavlink = require("mavlink_cmdint") + +local location_tracker + +-- constrain a value between limits +local function constrain(v, vmin, vmax) + if v < vmin then + v = vmin + end + if v > vmax then + v = vmax + end + return v +end + +local function set_avoid_mode(new_mode) + avoid_enter_mode = vehicle_mode + vehicle:set_mode(new_mode) +end + +local function reset_avoid_mode() + local new_mode = FLIGHT_MODE.AUTO + if avoid_enter_mode == FLIGHT_MODE.AUTO or avoid_enter_mode == FLIGHT_MODE.GUIDED or avoid_enter_mode == FLIGHT_MODE.LOITER or + avoid_enter_mode == FLIGHT_MODE.QHOVER or avoid_enter_mode == FLIGHT_MODE.QLOITER or avoid_enter_mode == FLIGHT_MODE.QRTL or + avoid_enter_mode == FLIGHT_MODE.RTL then + new_mode= avoid_enter_mode + end + vehicle:set_mode(new_mode) + avoid_enter_mode = -1 + + if new_mode == FLIGHT_MODE.AUTO then + previous_location = location_tracker.get_saved_location() + if previous_location ~= nil then + vehicle:set_crosstrack_start(previous_location) + end + end +end + +local function disable_wvane() + q_wvane_enable_save = Q_WVANE_ENABLE:get() + Q_WVANE_ENABLE:set(0) +end + +local function enable_wvane() + if q_wvane_enable_save >=0 then + Q_WVANE_ENABLE:set(q_wvane_enable_save) + else + Q_WVANE_ENABLE:set(0) + end + q_wvane_enable_save = -1 +end + +local airbrake_trigger = false +local airbrake_on = false +local airbrake_triggered_now = millis():tofloat() * 0.001 + +-- This method terminates airbraking if it had been acive +local function slowdown_airbrake_end() + airbrake_trigger = false + if not airbrake_on then + return + end + airbrake_on = false + 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 exited QLoiter, so we don't want to mess with that + return + end + reset_avoid_mode() +end + +local function slow_down(groundspeed) + local groundspeed_error = groundspeed.error or 0.0 + local airspeed_current = ahrs:airspeed_estimate() + local airspeed_new = constrain(airspeed_current + groundspeed_error, airspeed_min, airspeed_max) + + if groundspeed_airbrake_limit ~= 0 and not airbrake_on and groundspeed_error < groundspeed_airbrake_limit then + if not airbrake_trigger then + airbrake_trigger = true + airbrake_triggered_now = millis():tofloat() * 0.001 + end + + if (now - airbrake_triggered_now) > pitch_timeout then + airbrake_on = true + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": airbrake current %f error %f new %f", airspeed_current,groundspeed_error,airspeed_new) ) + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": airbraking Starting") + disable_wvane() + set_avoid_mode(FLIGHT_MODE.QLOITER) + end + elseif airbrake_trigger then + airbrake_trigger = false + end + if airbrake_on then + if groundspeed_error >= 0 or (now - airbrake_triggered_now) > 10 then -- it's not working, give up + slowdown_airbrake_end() + else + -- we want to hover in place + THROTTLE_CHANNEL:set_override(1500) + end + else + mavlink.set_vehicle_speed({speed=airspeed_new}) + end +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 + local target_alt_cm = math.floor(math.min(new_altitude * 100, 319000)) + new_location_target:alt(target_alt_cm) + -- can't use MAVLink for this because we might not be in GUIDED mode + if not vehicle:update_target_location(current_location_target,new_location_target) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. + string.format(": failed to set alt(cm): %.0f frame: %d current frame: %d", target_alt_cm, new_location_target:get_alt_frame(), current_location_target:get_alt_frame())) + end + else + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": failed to set alt: " .. new_altitude) + end + end + return new_altitude +end + +-- This method forces the plane to target a very high altitude to force it to gain altitude quicky +local function set_altitude_high() + local target_altitude = current_altitude + 100 + local old_target_altitude = 0.0 + if current_location_target == nil and current_location ~= nil then + old_target_altitude = current_location:alt() * 0.01 + elseif current_location_target ~= nil then + old_target_altitude = current_location_target:alt() * 0.01 + end + set_altitude_target(target_altitude) + return old_target_altitude +end + +-- Attempts to duplicate the code that updates the prev_WP_loc variable in the c++ code +local function LocationTracker() + + local self = {} + + -- to get this to work, need to keep 2 prior generations of "target_location" + local target_location + local previous_target_location -- the target prior to the current one + local previous_previous_target_location -- the target prior to that - this is the one we want + + function self.same_loc_as(A, B) + if A == nil or B == nil then + return false + end + if (A:lat() ~= B:lat()) or (A:lng() ~= B:lng()) then + return false + end + return (A:alt() == B:alt()) and (A:get_alt_frame() == B:get_alt_frame()) + end + + function self.update() + target_location = vehicle:get_target_location() + if target_location ~= nil then + if not self.same_loc_as(previous_target_location, target_location) then + -- maintain three generations of location + if previous_target_location ~= nil then + previous_previous_target_location = previous_target_location:copy() + end + previous_target_location = target_location:copy() + --gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. + -- string.format(": saved loc lat %.1f lng %.1f alt %.1f", target_location:lat(), target_location:lng(), target_location:alt())) + end + else + previous_target_location = ahrs:get_location() + previous_previous_target_location = previous_target_location + end + end + + function self.get_saved_location() + return previous_previous_target_location + end + + return self +end + +location_tracker = LocationTracker() + +local function start_cmtc(target_alt_amsl) + if cmtc_active then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": CMTC to ALREADY ACTIVE: " .. cmtc_target_alt_amsl) + return + end + cmtc_active = true + + local direction = avoid_terrain(target_alt_amsl) + + -- AP libraries use a 0.5m "near enough" buffer for matching altitude, we'll use 3 meters + cmtc_target_alt_amsl = target_alt_amsl - 3.0 + + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. string.format(": CMTC loiter to the %s to %.0f alt", direction, cmtc_target_alt_amsl) ) +end + +local function stop_cmtc() + if current_location ~= nil then + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. string.format(": CMTC Done alt: %.0f", current_location:alt() * 0.01) ) + end + cmtc_active = false + cmtc_target_alt_amsl = -1 + reset_avoid_mode() +end + +local function do_cmtc() + if current_location ~= nil then + local current_location_amsl = current_location:copy() + current_location_amsl:change_alt_frame(mavlink.ALT_FRAME.ABSOLUTE) + if current_location_amsl:alt() * 0.01 > cmtc_target_alt_amsl then + stop_cmtc() + end + end +end + +local function start_pitching() + if slowdown_quading then + start_quading() -- we are already in multirotor mode, so go straight to quading + return + end + + pitching_active = true + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": Pitching started") + + -- save the current target location so we can track if we pass a waypoint while pitching + pre_pitch_mode = vehicle_mode + pre_pitch_target_altitude = set_altitude_high() + pre_pitch_target_location = current_location_target:copy() + + -- pitch up by setting a very high altitude and high speed. TECS will make it so. + local desired_airspeed = 30 + if AIRSPEED_MAX ~= nil then + desired_airspeed = AIRSPEED_MAX:get() + end + mavlink.set_vehicle_speed({speed=desired_airspeed}) + + pitch_last_bad_timestamp = millis():tofloat() * 0.001 +end + +local function check_pitching() + if ground_speed ~= nil and ground_speed < pitch_groundspeed_min then + return false + end + + if pitch_obstacle_detected(1.0) then + if not pitching_active then + -- we don't jump into pitching right away, we give it a ZTA_PTCH_TIMEOUT seconds to be sure + local time_since_good = (now - pitch_last_good_timestamp) + if time_since_good > pitch_timeout then + start_pitching() + if pitch_obstacle_down(1.0) then + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. string.format(": Obstacle down: %.2f m", rangefinder_down_value) ) + end + if pitch_obstacle_forward(1.0) then + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. string.format(": Obstacle forward: %.2f m", rangefinder_forward_value) ) + end + else + if time_since_good > pitch_bad_timer + 1 then + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": high terrain detected") + pitch_bad_timer = math.floor(time_since_good + 0.5) + end + end + end + return true + else + if pitch_bad_timer >= 0 and not terrain_max_exceeded then + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": terrain Ok") + end + pitch_last_good_timestamp = now + pitch_bad_timer = -1 + end + return false +end + +local function stop_pitching() + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": Pitching DONE") + pitching_active = false + if pre_pitch_mode ~= vehicle_mode then + return -- user or maybe a failsafe changed mdoes. Don't interfere + end + if pre_pitch_target_altitude ~= nil then + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": reset Alt: " .. pre_pitch_target_altitude) + set_altitude_target(pre_pitch_target_altitude) + speed_PI.reset(0) + mavlink.set_vehicle_speed({}) -- special airspeed value meaning "default" + previous_location = location_tracker.get_saved_location() + if previous_location ~= nil then + vehicle:set_crosstrack_start(previous_location) + end + else + -- don't know where to go, so lets go home (like a good kid) + vehicle:set_mode(FLIGHT_MODE.RTL) + end +end + +local function do_pitching() + -- quading takes precedence over pitching + if quading_active then + pitching_active = false + return + end + + -- if we are above the pitching altitude (with a margin) we might want to stop pitching + -- we don't stop pitching right away, we give it a ZTA_PTCH_TIMEOUT seconds * 2 to be sure + if not pitch_obstacle_detected(PITCH_TOLERANCE) and (now - pitch_last_bad_timestamp) > (pitch_timeout * 2) or + not (pre_pitch_target_location:lat() == current_location_target:lat() and pre_pitch_target_location:lng() == current_location_target:lng() ) then + stop_pitching() + else + set_altitude_high() + pitch_last_bad_timestamp = millis():tofloat() * 0.001 + end +end + +start_quading = function() -- forward declaration above + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": Quading started") + if slowdown_quading then + slowdown_quading = false -- already in multi-motor mode, so just switch to quading + else + set_avoid_mode(FLIGHT_MODE.QLOITER) + disable_wvane() + end + quading_active = true + pitching_active = false + THROTTLE_CHANNEL:set_override(2000) +end + +local function check_quading() + if quad_obstacle_detected() then + if not quading_active then + if quad_obstacle_down() then + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. string.format(": Obstacle down: %.2f m", rangefinder_down_value) ) + end + if quad_obstacle_forward() then + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. string.format(": Obstacle forward: %.2f m", rangefinder_forward_value) ) + end + if pitching_active then + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": Stop Pitching") + stop_pitching() + end + start_quading() + return true + end + end + return false +end + +stop_quading = function() -- forward declaration above + if quading_active then + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": Quading DONE") + end + quading_active = false + airbrake_on = false + speed_PI.reset(0) + enable_wvane() + if vehicle_mode ~= FLIGHT_MODE.QLOITER then + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": exit Quading NOT QLoiter") + avoid_enter_mode = 0 + return -- user or failsafe has changed modes. Don't interfere + end + reset_avoid_mode() +end + +local function do_quading() + -- not a typo, if we kick into quading we want to go higher than would trigger pitching when when we are done + if not pitch_obstacle_detected(PITCH_TOLERANCE) or terrain_max_exceeded then + -- we are done quadding + stop_quading() + else + -- continually override the throttle to keep going up + THROTTLE_CHANNEL:set_override(2000) + end +end + +-- This function decides if there is an obstacle for pitching based on +-- 1. if terrain height is < pitch_down_min or +-- 2. if there is a valid rangefinder down and rangefinder_down_value < pitch_down_min or +-- 3. if there is a valid rangefinder forward and rangefinder_forward_value < pitch_forward_min +-- +pitch_obstacle_down = function(multiplier) + if rangefinder_down_value > 0 and rangefinder_down_value < MAX_RANGEFINDER_VALUE + and rangefinder_down_value < (pitch_down_min * multiplier) then + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": Obstacle Down " .. rangefinder_down_value .. " below: " .. (pitch_down_min * multiplier)) + + return true + end + return false +end +pitch_obstacle_forward = function(multiplier) + if rangefinder_forward_value > 0 and rangefinder_forward_value < MAX_RANGEFINDER_VALUE + and rangefinder_forward_value < (pitch_forward_min * multiplier) then + return true + end + return false +end + +pitch_obstacle_detected = function(multiplier) + -- prevent pitching if we are in a flyaway situation + if terrain_max_exceeded then + if pitching_active then + gcs:send_text(MAV_SEVERITY.CRITICAL, SCRIPT_NAME_SHORT .. ": Pitching " .. terrain_altitude .. " above: " .. altitude_max) + end + if quading_active then + gcs:send_text(MAV_SEVERITY.CRITICAL, SCRIPT_NAME_SHORT .. ": Quading " .. terrain_altitude .. " above: " .. altitude_max) + end + if cmtc_active then + gcs:send_text(MAV_SEVERITY.CRITICAL, SCRIPT_NAME_SHORT .. ": CMTC " .. terrain_altitude .. " above: " .. altitude_max) + end + return false + end + + if pitch_obstacle_down(multiplier) then + return true + end + if pitch_obstacle_forward(multiplier) then + return true + end + + -- no obstacle, we are good + return false +end + + +-- This function decides if there is an obstacle for quading based on +-- 1. if terrain height is < quad_down_min or +-- 2. if there is a valid rangefinder down and rangefinder_down_value < pitch_quad_min or +-- 3. if there is a valid rangefinder forward and rangefinder_forward_value < quad_forward_min +-- +function quad_obstacle_forward() + if rangefinder_forward_value > 0 and rangefinder_forward_value < MAX_RANGEFINDER_VALUE + and rangefinder_forward_value < quad_forward_min then + return true + end +end + +function quad_obstacle_down() + if rangefinder_down_value > 0 and rangefinder_down_value < MAX_RANGEFINDER_VALUE + and rangefinder_down_value < quad_down_min then + return true + end + return false +end + +function quad_obstacle_detected() + -- prevent quading if we are in a flyaway situation + if terrain_max_exceeded then + if quading_active then + gcs:send_text(MAV_SEVERITY.CRITICAL, SCRIPT_NAME_SHORT .. ": Quading " .. terrain_altitude .. " above: " .. altitude_max) + end + return false + end + + if quad_obstacle_down() then + return true + end + if quad_obstacle_forward() then + return true + end + + -- no obstacle, we are good + return false +end + +-- 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 + -- We'll probably want some kind of certainty check for the range finders + -- So a small error won't cause it to freakout. + + if rangefinder:has_data_orient(RANGEFINDER_ORIENT.DOWNWARD) + and rangefinder:status_orient(RANGEFINDER_ORIENT.DOWNWARD) == RANGEFINDER_STATUS.GOOD then + rangefinder_down_value = rangefinder:distance_cm_orient(RANGEFINDER_ORIENT.DOWNWARD) * 0.01 + else + -- if we don't have a downward rangefinder revert to terrain altitude + rangefinder_down_value = terrain:height_above_terrain(true) or 0.0 + end + if rangefinder:has_data_orient(RANGEFINDER_ORIENT.FORWARD) + and rangefinder:status_orient(RANGEFINDER_ORIENT.FORWARD) == RANGEFINDER_STATUS.GOOD then + rangefinder_forward_value = rangefinder:distance_cm_orient(RANGEFINDER_ORIENT.FORWARD) * 0.01 + else + rangefinder_forward_value = 0.0 + end + + terrain_altitude = terrain:height_above_terrain(true) + terrain_max_exceeded = false + if altitude_max ~= nil and terrain_altitude ~= nil then + 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 + rangefinder_down_value > MAX_RANGEFINDER_VALUE then + rangefinder_down_value = terrain_altitude or 0 + end + if rangefinder_forward_value == nil or rangefinder_forward_value <= 0 then + rangefinder_forward_value = 0 + end + if rangefinder_forward_value > MAX_RANGEFINDER_VALUE then + rangefinder_forward_value = MAX_RANGEFINDER_VALUE + end +end + +local function wrap_360(angle) + local res = math.fmod(angle, 360.0) + if res < 0 then + res = res + 360.0 + end + return res +end + +--[[ +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); + loc.offset_bearing(bearing, grid_spacing); + climb += climb_ratio * grid_spacing; + distance -= grid_spacing; + float height; + if (height_amsl(loc, height)) { + float rise = (height - base_height) - climb; + //if(rise > 0) + gcs().send_text(MAV_SEVERITY_INFO, "lookahead alt %.1f climb %.1f rise %.1f", height, climb, rise); + if (rise > lookahead_estimate) { + lookahead_estimate = rise; + loc_highest = loc; + gcs().send_text(MAV_SEVERITY_INFO, "lookahead estimate %.1f", lookahead_estimate); + } + } +} +--]] + +local function terrain_lookahead(start_location, search_bearing, search_distance, search_ratio) + local highest_location = nil + local climb = 0.0 + local highest_rise = 0.0 + local height + local search_location = start_location:copy() + search_location:change_alt_frame(mavlink.ALT_FRAME.ABSOLUTE) + + local base_height = terrain:height_amsl(search_location, true) + + while search_distance > 0 do + search_location:offset_bearing(search_bearing, terrain_spacing) + climb = climb + search_ratio * terrain_spacing + + height = terrain:height_amsl(search_location, true) + if height ~= nil then + local rise = (height - base_height) - climb + if rise > highest_rise then + local highest_alt_cm = math.floor(math.min(height * 100.0, 320000)) + highest_rise = rise + highest_location = search_location:copy() + highest_location:alt(highest_alt_cm) + end + end + + search_distance = search_distance - terrain_spacing + end + + return highest_location +end + +-- 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") ) + return + end + local wp_distance = current_location:get_distance(current_location_target) + local wp_bearing = math.deg(current_location:get_bearing(current_location_target)) + local pitch_required = 0 + local alt_required_amsl = -1 + local highest_location + local highest_alt_difference = 0.0 + local current_location_amsl = current_location:copy() + current_location_amsl:change_alt_frame(mavlink.ALT_FRAME.ABSOLUTE) + + if wp_distance == nil then + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": wp_distance NIL") ) + return 0.0, -1.0, 0.0 + end + + highest_location = terrain_lookahead(current_location, wp_bearing, wp_distance, 0.5 * tecs_climb_max / ground_speed) + if highest_location == nil then + return 0.0, -1.0, 0.0 + end + + -- 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) + 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 + -- what is the pitch up requried to acheive that altitude? + local highest_distance = current_location_amsl:get_distance(highest_location) + pitch_required = math.deg(math.atan(altitude_difference,highest_distance)) + -- the target location we need to hit needs to be AMSL to ensure we fly above terrain + alt_required_amsl = highest_location:alt() * 0.01 + highest_alt_difference = altitude_difference + end + + return pitch_required, alt_required_amsl, highest_alt_difference +end + +local function highest_arc_terrain(loiter_center, bearing_start, bearing_step, arc_max) + local next_increment = bearing_step + local highest_terrain = 0.0 + while math.abs(next_increment) < arc_max do + local test_bearing = wrap_360(bearing_start + next_increment) + + local loiter_edge = loiter_center:copy() + loiter_edge:offset_bearing(test_bearing, wp_loiter_rad) + local terrain_height = terrain:height_amsl(loiter_edge, true) + if terrain_height > highest_terrain then + highest_terrain = terrain_height or 0.0 + end + next_increment = next_increment + bearing_step + end + + return highest_terrain +end + +-- avoids upcoming terrain entering a loiter to altitude +-- loiters either left or right depending on which is less likely to hit terrain +avoid_terrain = function(target_alt_amsl) -- forward declaration above + -- calculate the highest location in an arc assuming that we loiter first right and then left + -- then we choose the one that has the lowest terrain either way + if current_location == nil then + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT ..": avoid_terrain no current_location") + return + end + --gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT ..": avoid_terrain: " .. target_alt_amsl) + local direction + local loiter_center_left = current_location:copy() + local loiter_center_right = current_location:copy() + loiter_center_left:offset_bearing(wrap_360(current_bearing - 90), wp_loiter_rad) + loiter_center_right:offset_bearing(wrap_360(current_bearing + 90), wp_loiter_rad) + -- at a radius of WP_LOITER_RAD how many degrees is TERRAIN_SPACING + -- Central Angle = Arc length(AB) / Radius(OA) = (s × 360°) / 2πr + local spacing_degrees = (terrain_spacing * 360.0) / (2.0 * math.pi * wp_loiter_rad) + + local highest_left_terrain = highest_arc_terrain(loiter_center_left, current_bearing, -spacing_degrees, 180) + local highest_right_terrain = highest_arc_terrain(loiter_center_right, current_bearing, spacing_degrees, 180) + local roll = ahrs:get_roll() + + set_avoid_mode(FLIGHT_MODE.GUIDED) + + -- loiter up to the requjired AMSL height, in the direction of lowest terrain + if highest_left_terrain < highest_right_terrain or roll < -30 then + mavlink.set_vehicle_target_location({lat = loiter_center_left:lat(), + lng = loiter_center_left:lng(), + alt = target_alt_amsl, + alt_frame = mavlink.ALT_FRAME.ABSOLUTE, + yaw = 1 }) + direction = "left" + else + mavlink.set_vehicle_target_location({lat = loiter_center_right:lat(), + lng = loiter_center_right:lng(), + alt = target_alt_amsl, + alt_frame = mavlink.ALT_FRAME.ABSOLUTE, + yaw = -1 }) + direction = "right" + end + + -- Set an extra hight altitude to ensure the plane tries to climb as fast as possible + mavlink.set_vehicle_target_altitude({alt = target_alt_amsl + 100.0, alt_frame = mavlink.ALT_FRAME.ABSOLUTE}) + mavlink.set_vehicle_speed({speed=airspeed_max}) + + return direction +end + +local switch_on = true +local last_switch_state = -1 + +local function activate() + switch_on = true + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT ..": activated") + pitch_last_good_timestamp = now +end + +local function deactivate() + switch_on = false + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT ..": deactivated") +end + + +-- This method checks if the activation switch has been newly enabled or disabled +local function check_activation_switch() + local switch_function = ZTA_ACT_FN:get() + if switch_function == nil then + return + end + local switch_state = rc:get_aux_cached(switch_function) or -1 + 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 High to turn off + deactivate() + end + -- Don't know what to do with the 3rd switch position right now. + last_switch_state = switch_state + end +end + +-- this method checks that all the conditions for terrain avoidance are met +local close_to_home = false +local function terravoid_active() + if not switch_on then + return false + end + if not (arming:is_armed()) then + return false + end + if not(vehicle_mode == FLIGHT_MODE.AUTO or vehicle_mode == FLIGHT_MODE.GUIDED or + vehicle_mode == FLIGHT_MODE.RTL or vehicle_mode == FLIGHT_MODE.QRTL or + ((quading_active or airbrake_on) and (vehicle_mode == FLIGHT_MODE.QLOITER or vehicle_mode == FLIGHT_MODE.QHOVER))) then + return false + end + + local home = ahrs:get_home() + local home_distance = 0.0 + if home ~= nil and current_location ~= nil then + home_distance = home:get_distance(current_location) or 0.0 + end + if home_distance ~= nil and home_distance_max ~= nil and home_distance < home_distance_max then + if not close_to_home then + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": close to home") + close_to_home = true + end + return false + end + if close_to_home then + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": away from home") + end + close_to_home = false + + return true +end + +local old_now = millis():tofloat() * 0.001 + +-- main update function called by protected_wrapper REFRESH_RATE times per second +local function update() + now = millis():tofloat() * 0.001 + current_location = ahrs:get_location() + if current_location ~= nil then + current_altitude = current_location:alt() * 0.01 + end + vehicle_mode = vehicle:get_mode() + current_location_target = vehicle:get_target_location() + current_bearing = vehicle:get_wp_bearing_deg() or -1 + groundspeed_vector = ahrs:groundspeed_vector() + ground_speed = ahrs:groundspeed_vector():length() + + -- save the previous target location only if in auto mode, if restoring it in AUTO mode + -- don't update it if already pitching or quading because the altitude change will mess up the history + if vehicle_mode == FLIGHT_MODE.AUTO and location_tracker ~= nil and not (pitching_active or quading_active) then + location_tracker.update() + end + + -- make the pitching and quading parameters updatable in the air (refresh every second) + if math.floor(old_now) ~= math.floor(now) then + pitch_groundspeed_min = ZTA_PTCH_GSP_MIN:get() + pitch_down_min = ZTA_PTCH_DWN_MIN:get() + pitch_forward_min = ZTA_PTCH_FWD_MIN:get() + pitch_timeout = ZTA_PTCH_TIMEOUT:get() + home_distance_max = ZTA_HOME_DIST:get() + + quad_down_min = ZTA_QUAD_DWN_MIN:get() + quad_forward_min = ZTA_QUAD_FWD_MIN:get() + + altitude_max = ZTA_ALT_MAX:get() + + groundspeed_max = ZTB_GSP_MAX:get() or -1 + 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 + stop_pitching() + end + pitching_active = false + quading_active = false + cmtc_active = false + return + end + + populate_rangefinder_values() + + -- first decide if we are seriously close to the terrain and need to start quading + if not quading_active and not check_quading() then + if cmtc_enable ==1 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(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 ZTB_CMTC_ALT clearance + start_cmtc(alt_required_amsl + cmtc_alt_m) + end + end + if not cmtc_active then + -- otherwise - lets see if we are close enough to need to start pitching + check_pitching() + end + end + + if terrain_max_exceeded and not cmtc_active then + if quading_active then + stop_quading() + end + if pitching_active then + stop_pitching() + end + end + + -- quading is the priority. If we are quading, do that, otherwise if we are pitching do that + if quading_active then + do_quading() + return + elseif pitching_active then + do_pitching() + return + elseif cmtc_active then + do_cmtc() + return + end + + -- in "normal" flight we want to maintain a minimum forward groundspeed + if groundspeed_vector == nil then + gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s: no groundspeed", SCRIPT_NAME_SHORT) ) + else + local groundspeed_current = groundspeed_vector:length() or 0 + if groundspeed_max > 0 and groundspeed_current > groundspeed_max then + -- if the groundspeed is too hign we need to slow down + slow_down({error = (groundspeed_max - groundspeed_current)}) + elseif groundspeed_airbrake_limit ~= 0 then + slowdown_airbrake_end() + mavlink.set_vehicle_speed({}) -- special airspeed value meaning "default" + end + end + +end + +-- wrapper around update(). This calls update() at 1/REFRESHRATE Hz, +-- and if update faults then an error is displayed, but the script is not +-- stopped +local function protected_wrapper() + local success, err = pcall(update) + if not success then + gcs:send_text(0, SCRIPT_NAME_SHORT .. ": Error: " .. err) + -- when we fault we run the update function again after 1s, slowing it + -- down a bit so we don't flood the console with errors + return protected_wrapper, 1000 + end + return protected_wrapper, 1000 * REFRESH_RATE +end + +local function delayed_startup() + gcs:send_text(MAV_SEVERITY.INFO, string.format("%s %s script loaded", SCRIPT_NAME, SCRIPT_VERSION) ) + return protected_wrapper() +end + +-- wait a bit for AP to come up then start running update loop +if FWVersion:type() == 3 and q_enable == 1 and terrain_enable == 1 then + return delayed_startup, 1000 * STARTUP_DELAY +else + gcs:send_text(MAV_SEVERITY.NOTICE,string.format("%s: Must run on QuadPlane with terrain follow", SCRIPT_NAME_SHORT)) +end diff --git a/libraries/AP_Scripting/applets/quadplane_terrain_avoid.md b/libraries/AP_Scripting/applets/quadplane_terrain_avoid.md new file mode 100644 index 0000000000000..b7a771f5c14b8 --- /dev/null +++ b/libraries/AP_Scripting/applets/quadplane_terrain_avoid.md @@ -0,0 +1,106 @@ +# QuadPlane Terrain Avoidance + + This script will detect if a quadplane following an Auto mission is likely to hit elevated terrain, such as + a small hill, cliff edge, high trees or other obstacles that might not show up in the OOTB STRM terrain model. + The code will attempt to avoid the impact by:- + - "Pitching" up if the plane can safely fly over the obstacle + - "Quading" up by switching to QuadPlane loiter mode (Quading) and gaining altitude using VTOL motors + - "CMTC" - can't make that climb. If the approaching terrain is higher than the plane can climb based on the + configured PTCH_LIM_MAX_DEG (divided by 2.0), then loiter to altitude before continuing. + +This code works best long range rangefinders such as the LightWare long range lidars that can measure + distances up to 90-95 meters away. + + The terrain avoidance will be on by default but will not function at "home" or within ZTA_HOME_DIST meters + of home. The scripting function ZTA_ACT_FN can be used to disable terrain folling at any time + Terrain following will operate in modes Auto, Gukded, RTL and QRTL. + +The "Can't make that climb" (CMTC) feature will prevent ArduPlane from flying into terrain it does know about +by calculating the required pitch to avoid terrain between the current location and the next waypoint including +all points in between. If the pitch required is > PTCH_LIM_MAX_DEG / 2 then the code will perform a loiter to +altitude, using fixed wing loiter, to acheive a safe altitude to avoid the terrain before continuing the mission. + +Note: Q_ASSIST should also be configured for best results ZTA_PTCH_DWN_MIN > Q_ASSIST_ALT > ZTA_QUAD_DWN_MIN + +# Parameters + +Beyond the normal Q_ASSIST parameters the script adds several additional parameters to +control it's behaviour. The parameters are prefixed with ZTA and ZTB. The parameters are: + +## ZTA_ACT_FN + +An RC scripting function to disable terrain avoidance. It defaults to on. + +## ZTA_HOME_DIST + +A circle around home (in meters), where terrain avoidance will not run. +Allows for safe takeoff and landing at the home location. + +## ZTA_ALT_MAX + +Maximum altitude above terrain that the plane can go to if avoiding terrain. +Set this to avoid "flyways" usually caused by malfunctioning rangefinders. + +## ZTA_PTCH_DWN_MIN + +The minimum distance to the ground directly when pitching will start. + +# ZTA_PTCH_FWD_MIN + +The minimum distance forward where pitching will start. This requires +a forward facing range finder best installed pointing at a 45 degree +angle. + +# ZTA_PTCH_GSP_MIN + +The minimum groundspeed to use pitching. If groundspeed is below this then +pitching will not be attempted. + +## ZTA_QUAD_DWN_MIN + +The minimum distance to the ground directly when quading will start. +Should be lower than ZTA_PTCH_DWN_MIN by at least 5m. + +# ZTA_QUAD_FWD_MIN + +The minimum distance forward where quadinging will start. This requires +a forward facing range finder best installed pointing at a 45 degree +angle. (the same one used by ZTA_PTCH_FWD_MIN) + +# ZTB_GSP_MAX + +The maximum groundspeed to attempt to fly. For best results when doing +magnetometry surveys ideally a steady groundspeed is required even +in windy conditions. This attempts to acheive that. + +# ZTB_GSP_AIRBRAKE + +If the vehicle exceeds ZTB_GSP_MAX and slowing the motors (desired airspeed) +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 +on TECS to do all the work. Some parameters it refers to directly. + +This script operates by default and can be turned off in flight with +a switch, or will disable it self if close to home (ZTA_HOME_DIST meters). + +Install the script in the APM/scripts folder on the SD card on your autopilot. Install the speedpi.lua +and mavlink_cmdint.lua modules in the APM/scripts/modules folder on the same SD card. Configure +the parmeters and set your transmitter with switch for ZTA_ACT_FN to allow disabling the function in the air. + + diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 660c1650fa18e..598ce9304c0df 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -3043,7 +3043,6 @@ function terrain:status() end ---@return boolean function terrain:enabled() end - -- RangeFinder state structure ---@class (exact) RangeFinder_State_ud local RangeFinder_State_ud = {} diff --git a/libraries/AP_Scripting/modules/mavlink_cmdint.lua b/libraries/AP_Scripting/modules/mavlink_cmdint.lua new file mode 100644 index 0000000000000..853c3738c7270 --- /dev/null +++ b/libraries/AP_Scripting/modules/mavlink_cmdint.lua @@ -0,0 +1,145 @@ +--[[ + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + This ArduPilot lua module should be in stalled in a "modules" subdirectory under the scripts + directory on your SD card or execution directory in SITL. + + This module provides wrapper functions for some commonly used Lua bindings + for MAVLink MAV_CMD command_int commands. + +--]] + +local MAVLink = {} + +MAVLink.SCRIPT_VERSION = "4.6.0-003" +MAVLink.SCRIPT_NAME = "MAVLink command_int module" +MAVLink.SCRIPT_NAME_SHORT = "MAVcmdint" + +MAVLink.SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} +-- these are the MAVLink altitude frames +MAVLink.FRAME = { GLOBAL = 0, GLOBAL_RELATIVE_ALT = 3, GLOBAL_RELATIVE_ALT_INT = 6, + GLOBAL_TERRAIN_ALT = 10, GLOBAL_TERRAIN_ALT_INT = 11} +-- these are the ArduPilot Location::ALtFrame frames +MAVLink.ALT_FRAME = { ABSOLUTE = 0, ABOVE_HOME = 1, ABOVE_ORIGIN = 2, ABOVE_TERRAIN = 3 } +MAVLink.CMD_INT = { DO_SET_MODE = 176, DO_CHANGE_SPEED = 178, DO_REPOSITION = 192, + GUIDED_CHANGE_SPEED = 43000, GUIDED_CHANGE_ALTITUDE = 43001, GUIDED_CHANGE_HEADING = 43002 } +MAVLink.SPEED_TYPE = { AIRSPEED = 0, GROUNDSPEED = 1, CLIMB_SPEED = 2, DESCENT_SPEED = 3 } +MAVLink.HEADING_TYPE = { COG = 0, HEADING = 1} -- COG = Course over Ground, i.e. where you want to go, HEADING = which way the vehicle points + +MAVLink.PLANE_FLIGHT_MODE = {AUTO=10, RTL=11, LOITER=12, GUIDED=15, QHOVER=18, QLOITER=19, QRTL=21} + +function MAVLink.NaN() + return 0/0 +end + +MAVLink.previous_speed = -1 + +function MAVLink.set_vehicle_speed(speed) + local new_speed = speed.speed or -2.0 -- special airspeed value meaning "default" + local speed_type = speed.type or MAV_SPEED_TYPE.AIRSPEED + local throttle = speed.throttle or 0.0 + local slew = speed.slew or 0.0 + + if new_speed == MAVLink.previous_speed then + return + end + MAVLink.previous_speed = new_speed + if FWVersion:type() == 3 and vehicle_mode == MAVLink.PLANE_FLIGHT_MODE.GUIDED and + speed_type == MAVLink.SPEED_TYPE.AIRSPEED and speed.slew ~= 0 then + local mavlink_result = gcs:run_command_int(MAVLink.CMD_INT.GUIDED_CHANGE_SPEED, { frame = MAVLink.FRAME.GLOBAL, + p1 = speed_type, + p2 = new_speed, + p3 = slew }) + if mavlink_result > 0 then + gcs:send_text(MAVLink.SEVERITY.ERROR, MAVLink.SCRIPT_NAME_SHORT .. string.format(": MAVLink GUIDED_CHANGE_SPEED returned %d", mavlink_result)) + return false + end + else + local mavlink_result = gcs:run_command_int(MAVLink.CMD_INT.DO_CHANGE_SPEED, { frame = MAVLink.FRAME.GLOBAL, + p1 = speed_type, + p2 = new_speed, + p3 = throttle }) + if mavlink_result > 0 then + gcs:send_text(MAVLink.SEVERITY.ERROR, MAVLink.SCRIPT_NAME_SHORT .. string.format(": MAVLink DO_CHANGE_SPEED returned %d", mavlink_result)) + return false + end + end + return true +end + +function MAVLink.set_vehicle_target_location(target) + local alt_frame = target.frame or MAVLink.ALT_FRAME.ABSOLUTE + local groundspeed = target.speed or -1 + local bitmask = target.bitmask or 0 + local radius = target.radius or 0 + local yaw = target.yaw or MAVLink.NaN() + local mavlink_result = gcs:run_command_int(MAVLink.CMD_INT.DO_REPOSITION, { + frame = MAVLink.alt_frame_to_mavlink(alt_frame), + p1 = groundspeed, + p2 = bitmask, + p3 = radius, + p4 = yaw, + x = target.lat, y = target.lng, z = target.alt}) + + if mavlink_result > 0 then + gcs:send_text(MAVLink.SEVERITY.ERROR, MAVLink.SCRIPT_NAME_SHORT .. string.format(": MAVLink DO_REPOSITION returned %d", mavlink_result)) + return false + end + return true +end + +function MAVLink.alt_frame_to_mavlink(alt_frame) + local mavlink_frame = MAVLink.FRAME.GLOBAL + if (alt_frame == MAVLink.ALT_FRAME.ABOVE_TERRAIN) then + mavlink_frame = MAVLink.FRAME.GLOBAL_TERRAIN_ALT + elseif (alt_frame == MAVLink.ALT_FRAME.ABOVE_HOME) then + mavlink_frame = MAVLink.FRAME.GLOBAL_RELATIVE_ALT + end + return mavlink_frame +end + +function MAVLink.mavlink_frame_to_alt(mavlink_frame) + local alt_frame = MAVLink.ALT_FRAME.ABSOLUTE + if (mavlink_frame == MAVLink.FRAME.GLOBAL_RELATIVE_ALT or + mavlink_frame == MAVLink.FRAME.GLOBAL_RELATIVE_ALT_INT) then + alt_frame = MAVLink.ALT_FRAME.ABOVE_HOME + elseif (mavlink_frame == MAVLink.FRAME.GLOBAL_TERRAIN_ALT or + mavlink_frame == MAVLink.FRAME.GLOBAL_TERRAIN_ALT_INT) then + alt_frame = MAVLink.ALT_FRAME.ABOVE_TERRAIN + end + return alt_frame +end + +function MAVLink.set_vehicle_target_altitude(target) + local velocity = target.velocity or 1000.0 -- default to maximum z acceleration + local alt = target.alt or nil + local alt_frame = target.alt_frame or MAVLink.ALT_FRAME.ABOVE_HOME + if alt == nil then + gcs:send_text(MAVLink.SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": set_vehicle_target_altitude no altiude") + return + end + -- GUIDED_CHANGE_ALTITUDE takes altitude in meters + local mavlink_result = gcs:run_command_int(MAVLink.CMD_INT.GUIDED_CHANGE_ALTITUDE, { + frame = MAVLink.alt_frame_to_mavlink(alt_frame), + p3 = velocity, + z = alt }) + if mavlink_result > 0 then + gcs:send_text(MAVLink.SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format(": MAVLink GUIDED_CHANGE_ALTITUDE returned %d", mavlink_result)) + return false + end + return true +end + +return MAVLink \ No newline at end of file diff --git a/libraries/AP_Scripting/modules/speedpi.lua b/libraries/AP_Scripting/modules/speedpi.lua new file mode 100644 index 0000000000000..60034062d7b00 --- /dev/null +++ b/libraries/AP_Scripting/modules/speedpi.lua @@ -0,0 +1,155 @@ +--[[ + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + SpeedPI + A simple "PI" controller for airspeed. Copied from Andrew Tridgell's original + work on the ArduPilot Aerobatics Lua scripts. + + Usage: + 1. drop it in the scripts/modules directory + 2. include in your own script using + local speedpi = requre("speedpi.lua") + 3. create an instance - you may need to tune the gains + local speed_controller = speedpi.speed_controller(0.1, 0.1, 2.5, airspeed_min, airspeed_max) + 4. call it's update() from your update() with the current airspeed and airspeed error + local airspeed_new = speed_controller.update(vehicle_airspeed, desired_airspeed - vehicle_airspeed) + 5. Set the vehicle airspeed based on the airspeed_new value returned from speedpi + +--]] + +local SpeedPI = {} + +SpeedPI.SCRIPT_VERSION = "4.6.0-004" +SpeedPI.SCRIPT_NAME = "Speed PI Controller" +SpeedPI.SCRIPT_NAME_SHORT = "SpeedPI" + +-- constrain a value between limits +function SpeedPI.constrain(v, vmin, vmax) + if v < vmin then + v = vmin + end + if v > vmax then + v = vmax + end + return v +end + +function SpeedPI.PI_controller(kP,kI,iMax,min,max) + -- the new instance. You can put public variables inside this self + -- declaration if you want to + local self = {} + + -- private fields as locals + local _kP = kP or 0.0 + local _kI = kI or 0.0 + local _iMax = iMax + local _min = min + local _max = max + local _last_t = nil + local _I = 0 + local _P = 0 + local _total = 0 + local _counter = 0 + local _target = 0 + local _current = 0 + local nowPI = millis():tofloat() * 0.001 + + -- update the controller. + function self.update(target, current) + local now = millis():tofloat() * 0.001 + if not _last_t then + _last_t = now + end + local dt = now - _last_t + _last_t = now + local err = target - current + _counter = _counter + 1 + + local P = _kP * err + if ((_total < _max and _total > _min) or + (_total >= _max and err < 0) or + (_total <= _min and err > 0)) then + _I = _I + _kI * err * dt + end + if _iMax then + _I = SpeedPI.constrain(_I, -_iMax, iMax) + end + local I = _I + local ret = target + P + I + if math.floor(now) ~= math.floor(nowPI) then + nowPI = millis():tofloat() * 0.001 + end + _target = target + _current = current + _P = P + + ret = SpeedPI.constrain(ret, _min, _max) + _total = ret + return ret + end + + -- reset integrator to an initial value + function self.reset(integrator) + _I = integrator + end + + function self.set_I(I) + _kI = I + end + + function self.set_P(P) + _kP = P + end + + function self.set_Imax(Imax) + _iMax = Imax + end + + -- log the controller internals + function self.log(name, add_total) + -- allow for an external addition to total + -- Targ = Current + error ( target airspeed ) + -- Curr = Current airspeed input to the controller + -- P = calculated Proportional component + -- I = calculated Integral component + -- Total = calculated new Airspeed + -- Add - passed in as 0 + logger.write(name,'Targ,Curr,P,I,Total,Add','ffffff',_target,_current,_P,_I,_total,add_total) + end + + -- return the instance + return self + end + +function SpeedPI.speed_controller(kP_param, kI_param, iMax, sMin, sMax) + local self = {} + local speedpi = SpeedPI.PI_controller(kP_param, kI_param, iMax, sMin, sMax) + + function self.update(spd_current, spd_error) + local adjustment = speedpi.update(spd_current + spd_error, spd_current) + speedpi.log("ZSPI", 0) -- Z = scripted, S = speed, PI = PI controller + return adjustment + end + + function self.reset() + speedpi.reset(0) + end + + return self +end + +gcs:send_text(MAV_SEVERITY.INFO, string.format("%s %s module loaded", SpeedPI.SCRIPT_NAME, SpeedPI.SCRIPT_VERSION) ) + +return SpeedPI \ No newline at end of file