libraries/AP_Scripting/examples/plane-callout-alt.lua - master - Sangak/ardupilot

libraries/AP_Scripting/examples/plane-callout-alt.lua
218 строк8 KB
Tim Tuxworth
AP_Scripting: Add Altitude Callout script to examples
23 ноя 2024, 23:17
23 ноя 2024, 23:171b4ac6a
--[[

   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 <http://www.gnu.org/licenses/>.

--]]

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