From 440908d3efbbe6b1af1c81abe525aa62b16eaf9b Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Thu, 30 Mar 2023 08:51:45 -0600 Subject: [PATCH] AP_Scripting: Add Altitude Callout script to examples --- .../examples/plane-callout-alt.lua | 219 ++++++++++++++++++ 1 file changed, 219 insertions(+) create mode 100644 libraries/AP_Scripting/examples/plane-callout-alt.lua diff --git a/libraries/AP_Scripting/examples/plane-callout-alt.lua b/libraries/AP_Scripting/examples/plane-callout-alt.lua new file mode 100644 index 0000000000000..4a7181dbad126 --- /dev/null +++ b/libraries/AP_Scripting/examples/plane-callout-alt.lua @@ -0,0 +1,219 @@ +--[[ + + 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 . + +--]] + +SCRIPT_VERSION = "4.6.0-003" +SCRIPT_NAME = "Plane Altitude Callouts" +SCRIPT_NAME_SHORT = "Callout" + +-- This script calls out altitude if the user is near (WARN) or above (MAX) +-- Allows for units to be feet even if you do everything else in metric because the laws typically specify 400 feet for UAV/RPAS in most countries +-- all of the settings are stored in parameters: +-- CALLOUT_ALT_UNITS 1 = metric, 2 (default) = imperial +-- CALLOUT_ALT_MAX max allowed altitude (its still a message there is no action) +-- CALLOUT_ALT_STEP callout (via GC message) when altitude changes by this amount or more +-- CALLOUT_ALT_CALL seconds between callout of flying altitude +-- CALLOUT_ALT_WARN seconds between callouts that you are less than ALT_STEP below ALT_MAX +-- CALLOUT_ALT_HIGH seconds between callouts that you have exceeded ALT_MAX + +REFRESH_RATE = 1000 --check every 1 second + +MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +local PARAM_TABLE_KEY = 102 +local PARAM_TABLE_PREFIX = "ZPC_" + +-- add a parameter and bind it to a variable +local function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return Parameter(PARAM_TABLE_PREFIX .. name) +end + +-- setup follow mode specific parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), 'could not add param table') + + +-- Add these ZPC_ parameters specifically for this script +--[[ + // @Param: ZPC_ALT_UNITS + // @DisplayName: Plane Callouts Units - defaults to feet + // @Description: 1: Metric/meters or 2: Imperial/feet + // @User: Standard +--]] +ZPC_ALT_UNITS = bind_add_param('ALT_UNITS', 1, 2) + +--[[ + // @Param: ZPC_ALT_MAX + // @DisplayName: Plane Callouts max altitude + // @Description: Maximum altitude in ALT_UNITS + // @Range: 0 30 + // @Units: seconds +--]] +ZPC_ALT_MAX = bind_add_param("ALT_MAX", 2, 400) + +--[[ + // @Param: ZPC_ALT_STEP + // @DisplayName: Plane Callouts altitude steps + // @Description: Altitude steps for callouts in ALT_UNITS + // @Range: 0 100 + // @Units: ALT_UNIT +--]] +ZPC_ALT_STEP = bind_add_param("ALT_STEP", 3, 25) + +--[[ + // @Param: ZPC_ALT_CALL + // @DisplayName: Plane Callouts frequency + // @Description: How often to callout altitude when flying normally + // @Range: 0 30 + // @Units: seconds +--]] +ZPC_ALT_CALL = bind_add_param("ALT_CALL", 4, 25) + +--[[ + // @Param: ZPC_ALT_WARN + // @DisplayName: Plane altitude warning frequency + // @Description: How often to nag about almost hitting MAX + // @Range: 0 30 + // @Units: seconds +--]] +ZPC_ALT_WARN = bind_add_param("ALT_WARN", 5, 25) + +--[[ + // @Param: ZPC_ALT_HIGH + // @DisplayName: Plane altitude max altitude callout frequency + // @Description: How often to nag about exceeding MAX + // @Range: 0 30 + // @Units: seconds +--]] +ZPC_ALT_HIGH = bind_add_param("ALT_HIGH", 6, 10) + +local alt_units = ZPC_ALT_UNITS:get() or 2 -- default to feet because "thats what it is" +local alt_max = ZPC_ALT_MAX:get() or 400 -- most places this is the legal limit, if you have a different limit, change this +local alt_step = ZPC_ALT_STEP:get() or 25 +local alt_call_sec = ZPC_ALT_CALL:get() or 25 +local alt_warn_sec = ZPC_ALT_WARN:get() or 25 +local alt_high_sec = ZPC_ALT_HIGH:get() or 10 + +local alt_last = 0 +local alt_warn = alt_max - alt_step + +local unit = "meters" +if (alt_units == 2) then + unit = "feet" +end +local altitude_max = string.format("%i %s", math.floor(alt_max+0.5), unit ) + +local time_last_warn_s = millis() / 1000 +local time_last_max_s = millis() / 1000 +local time_last_update_s = millis() / 1000 +local alt_max_exceeded = false +local alt_warn_exceeded = false + +local function update() -- this is the loop which periodically runs + local current_time_s = millis() / 1000 + -- setting the height/altitude variables like this means all the code below works without change for either metric or Imperial units + local terrain_height = terrain:height_above_terrain(true) + + -- if terrain height is not available use height above home + if terrain_height == nil then + -- override terrain height with home height (TODO: parameterize this) + local pos = ahrs:get_relative_position_NED_home() + if pos == nil then + return + else + terrain_height = -pos:z() + end + end + + --- allow these parameters to be changed at runtime. + alt_units = ZPC_ALT_UNITS:get() or 2 + alt_max = ZPC_ALT_MAX:get() or 400 -- most places this is the legal limit, if you have a different limit, change this + alt_step = ZPC_ALT_STEP:get() or 25 + alt_call_sec = ZPC_ALT_CALL:get() or 25 + alt_warn_sec = ZPC_ALT_WARN:get() or 25 + alt_high_sec = ZPC_ALT_HIGH:get() or 10 + if (alt_units == 2) then + unit = "feet" + else + unit = "meters" + end + + if (alt_units == 2) then + terrain_height = terrain_height * 3.28084 + end + local altitude = string.format("%i %s", math.floor(terrain_height+0.5), unit ) + + if terrain_height > alt_max then + if (time_last_max_s < current_time_s - alt_high_sec) then + gcs:send_text(MAV_SEVERITY.ERROR, string.format("Altitude %s too high max %s", altitude, altitude_max )) + time_last_max_s = current_time_s + alt_max_exceeded = true + end + elseif terrain_height > alt_warn then + if (time_last_warn_s < current_time_s - alt_warn_sec) then + gcs:send_text(MAV_SEVERITY.WARNING, string.format("Warning altitude is %s", altitude )) + time_last_warn_s = current_time_s + alt_warn_exceeded = true + end + else + -- we are fine now, but maybe we were not fine before. + -- So if we previously displayed altitude warn/error messages, let the pilot know we are now fine + if(alt_max_exceeded or alt_warn_exceeded) then + gcs:send_text(MAV_SEVERITY.WARNING, string.format("Altitude %s is Ok", altitude )) + else + -- nothing else important happened, so see if our altitude has gone up or down by more than ALT_STEP + -- in which case we call it out + if (time_last_update_s < current_time_s - alt_call_sec) then + local alt_diff = (terrain_height - alt_last) + if (math.abs(alt_diff) > alt_step) then + gcs:send_text(MAV_SEVERITY.WARNING, string.format("Altitude is %s", altitude )) + alt_last = math.floor(terrain_height / alt_step + 0.5) * alt_step + end + time_last_update_s = current_time_s + end + end + alt_max_exceeded = false + alt_warn_exceeded = false + end +end + +local displayed_banner = false +-- wrapper around update(). This calls update() at 1Hz, +-- and if update faults then an error is displayed, but the script is not +-- stopped +local function protected_wrapper() + + if not displayed_banner then + gcs:send_text(MAV_SEVERITY.INFO, string.format("%s %s script loaded callouts in %s", SCRIPT_NAME, SCRIPT_VERSION, unit) ) + displayed_banner = true + end + + if not arming:is_armed() then return protected_wrapper, REFRESH_RATE * 10 end + + local success, err = pcall(update) + if not success then + gcs:send_text(MAV_SEVERITY.ALERT, SCRIPT_NAME_SHORT .. "Internal 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, REFRESH_RATE +end + + + -- start running update loop - wait 20 seconds before starting up +return protected_wrapper, 20000 +