ArduPilot/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-02-04 12:25:35 -06:00
Code Issues Packages Projects Releases 82 Wiki Activity
ardupilot/libraries/AP_Scripting/applets/arming-checks.lua

635 lines
27 KiB
Lua
Raw Permalink Blame History

-- This script runs arming checks for validations that are important
-- to some, but might need to behave differently depending on the vehicle or use case
-- so we don't want to bake them into the firmware.
-- The severity of each check can be set by simply setting the value of parameters.
-- from warnings, which still allow arming to to errors failsafe which prevent arming
-- Set the value of the matching parameter to the MAV_SEVERITY of the error level you want.
--
-- New checks can optionally be added, by adding a true/false function (false = fail)
-- and adding a check to the arming_checks table.
--
-- Thanks to @yuri_rage and Peter Barker for help with the Lua and Autotests
SCRIPT_VERSION = "4.7.0-022"
SCRIPT_NAME = "Arming Checks"
SCRIPT_NAME_SHORT = "ArmCk"
REFRESH_DELAY = 500 -- wait this many milliseconds between each update loop (while not armed)
INITIAL_DELAY = 20000 -- wait 20 seconds for the AP to settle down before starting to show messages
GLOBAL_PARAM_TABLE_BASE = 170
VALUES_PARAM_TABLE_KEY = GLOBAL_PARAM_TABLE_BASE + 9
VALUES_PARAM_TABLE_PREFIX = "ARM_V_"
FIRMWARE = {GLOBAL = 0, ROVER = 1, COPTER = 2, PLANE = 3, ANTENNA = 4, SUB = 7, BLIMP = 12, HELI = 13 }
VEHICLE = {
-- these table key comments are so that a grep on PARAM_TABLE_KEY will find all the keys used by this script
-- PARAM_TABLE_KEY = 170
[FIRMWARE.GLOBAL] = {prefix = "ARM_", key = GLOBAL_PARAM_TABLE_BASE},
-- PARAM_TABLE_KEY = 171
[FIRMWARE.ROVER] = {prefix = "ARM_R_", key = GLOBAL_PARAM_TABLE_BASE + 1},
-- PARAM_TABLE_KEY = 172
[FIRMWARE.COPTER] = {prefix = "ARM_C_", key = GLOBAL_PARAM_TABLE_BASE + 2},
-- PARAM_TABLE_KEY = 173
[FIRMWARE.PLANE] = {prefix = "ARM_P_", key = GLOBAL_PARAM_TABLE_BASE + 3},
-- PARAM_TABLE_KEY = 174
[FIRMWARE.ANTENNA] = {prefix = "ARM_A_", key = GLOBAL_PARAM_TABLE_BASE + 4},
-- PARAM_TABLE_KEY = 175
[FIRMWARE.BLIMP] = {prefix = "ARM_B_", key = GLOBAL_PARAM_TABLE_BASE + 5},
-- PARAM_TABLE_KEY = 176
[FIRMWARE.HELI] = {prefix = "ARM_H_", key = GLOBAL_PARAM_TABLE_BASE + 6},
}
MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7, NONE=-1}
MAV_SEVERITY_TEXT = {[MAV_SEVERITY.EMERGENCY] = "emrg",
[MAV_SEVERITY.ALERT] = "alrt",
[MAV_SEVERITY.CRITICAL] = "crit",
[MAV_SEVERITY.ERROR] = "fail",
[MAV_SEVERITY.WARNING] = "warn",
[MAV_SEVERITY.NOTICE] = "note",
[MAV_SEVERITY.INFO] = "info",
[MAV_SEVERITY.DEBUG] = "debg",
}
PLANE_MODE = { AUTO = 10, TAKEOFF = 13}
-- create parameter table for general/global ARM_ parameters that will be created below.
assert(param:add_table(VEHICLE[FIRMWARE.GLOBAL].key, VEHICLE[FIRMWARE.GLOBAL].prefix, 20), string.format('could not add param table: (%d) %s ', VEHICLE[FIRMWARE.GLOBAL].key, VEHICLE[FIRMWARE.GLOBAL].prefix))
-- create parameter table for ARM_C_ parameters for copter that will be created below.
assert(param:add_table(VEHICLE[FIRMWARE.COPTER].key, VEHICLE[FIRMWARE.COPTER].prefix, 10), string.format('could not add param table: (%d) %s ', VEHICLE[FIRMWARE.COPTER].key, VEHICLE[FIRMWARE.COPTER].prefix))
-- create parameter table for ARM_P_ parameters for plane that will be created below.
assert(param:add_table(VEHICLE[FIRMWARE.PLANE].key, VEHICLE[FIRMWARE.PLANE].prefix, 10), string.format('could not add param table: (%d) %s ', VEHICLE[FIRMWARE.PLANE].key, VEHICLE[FIRMWARE.PLANE].prefix))
-- create parameter table for ARM_V_ VALUE parameters which are "reference values" used by the parameter methods
assert(param:add_table(VALUES_PARAM_TABLE_KEY, VALUES_PARAM_TABLE_PREFIX, 15), string.format('could not add param table: (%d) %s ', VALUES_PARAM_TABLE_KEY, VALUES_PARAM_TABLE_PREFIX))
local arm_auth_id = arming:get_aux_auth_id()
if arm_auth_id == nil then
gcs:send_text(MAV_SEVERITY.ERROR, string.format("%s %s Failed", SCRIPT_NAME, SCRIPT_VERSION) )
gcs:send_text(MAV_SEVERITY.ERROR, string.format("%s: get_aux_auth_id failed", SCRIPT_NAME_SHORT))
error(SCRIPT_NAME_SHORT .. ": failed to get arm auth ID")
return -- don't execute the script if we can't get an auth_id
end
---- CLASS: Arming_Check ----
local Arming_Check = {}
Arming_Check.__index = Arming_Check
setmetatable(Arming_Check, {
__call = function(cls, firmware, index, func, param_name, text, severity, passed, changed) -- constructor
local self = setmetatable({}, cls)
self.firmware = firmware -- which firmware type is this check for (0 for general/global)
self.index = index -- the index of the ardupilot parameter
self.func = func -- func() is called to validate arming
self.param_name = param_name -- the name of a ARM_ parameter overriding the severity
self.text = text -- the message on failure
self.severity = severity -- is failure a warning or hard stop?
self.passed = passed or nil
self.changed = changed or true
return self
end
})
function Arming_Check:state()
local passed_new = self.func()
if self.passed == passed_new then
self.changed = false
else
self.passed = passed_new
self.changed = true
end
return passed_new
end
-- parameters that store values used by the validation methods, rather than severities of the individual arming checks
ARM_V_ALT_LEGAL = Parameter()
local alt_legal_max = 120.0
FENCE_TYPE = Parameter("FENCE_TYPE")
FENCE_TOTAL = Parameter("FENCE_TOTAL")
FENCE_ENABLE = Parameter("FENCE_ENABLE")
FENCE_AUTOENABLE = Parameter("FENCE_AUTOENABLE")
FOLL_ENABLE = Parameter("FOLL_ENABLE")
FOLL_SYSID = Parameter("FOLL_SYSID")
RALLY_LIMIT_KM = Parameter("RALLY_LIMIT_KM")
if FWVersion:type() == FIRMWARE.COPTER then -- Copter specific paramters
RTL_ALTITUDE = Parameter("RTL_ALT_M")
RTL_CLIMB_MIN = Parameter("RTL_CLIMB_MIN_M")
elseif FWVersion:type() == FIRMWARE.PLANE then -- Plane specific Parameters
RTL_ALTITUDE = Parameter("RTL_ALTITUDE")
RTL_CLIMB_MIN = Parameter("RTL_CLIMB_MIN")
Q_ENABLE = Parameter("Q_ENABLE")
AIRSPEED_STALL = Parameter("AIRSPEED_STALL")
AIRSPEED_MIN = Parameter("AIRSPEED_MIN")
AIRSPEED_CRUISE = Parameter("AIRSPEED_CRUISE")
AIRSPEED_MAX = Parameter("AIRSPEED_MAX")
SCALING_SPEED = Parameter("SCALING_SPEED")
end
-- this is the pre 4.7 name for the SYSID parameter
---@diagnostic disable-next-line:lowercase-global
function try_sysid_thismav()
MAV_SYSID = Parameter("SYSID_THISMAV")
end
if not pcall(try_sysid_thismav) then
MAV_SYSID = Parameter("MAV_SYSID")
end
local validate -- forward definition of the validate() function
-- check that the MAVLink SYSID has been set for this vehicle
local function sysid_set()
local mav_sysid = MAV_SYSID:get()
if mav_sysid == nil or mav_sysid <= 1 then
return false
end
return true
end
-- check that if Follow is enabled, the FOLL_SYSID is set
local function foll_sysid_set()
local foll_enable = FOLL_ENABLE:get()
if foll_enable ~= 1 then
return true
end
local foll_sysid = FOLL_SYSID:get()
if foll_sysid == 0 then
return false
end
return true
end
-- check that if the FOLL_SYSID is set it's not pointing to itself (the test returns true if "good" - hence the _not_)
local function foll_sysid_not_thismav()
local foll_enable = FOLL_ENABLE:get()
if foll_enable ~= 1 then
return true
end
local mav_sysid = MAV_SYSID:get()
local foll_sysid = FOLL_SYSID:get()
if foll_sysid > 0 and mav_sysid == foll_sysid then
return false
end
return true
end
-- if FOLLOW is enabled make sure the XYZ offsets have been set
local function foll_ofs_default()
local foll_enable = FOLL_ENABLE:get()
if foll_enable ~= 1 then
return true
end
local foll_ofs_x = Parameter("FOLL_OFS_X"):get()
local foll_ofs_y = Parameter("FOLL_OFS_Y"):get()
local foll_ofs_z = Parameter("FOLL_OFS_Z"):get()
if foll_ofs_x == 0 and foll_ofs_y == 0 and foll_ofs_z == 0 then
return false
end
return true
end
-- for many use cases if MNTx_SYSID_DEFLT is set is should match the FOLL_SYSID (remember set ARM_MNTX_SYSID severity to NONE to disable this check)
local function mntx_sysid_match()
local foll_enable = FOLL_ENABLE:get()
local mnt1_enable = (Parameter("MNT1_TYPE"):get() ~= 0)
local mnt2_enable = (Parameter("MNT2_TYPE"):get() ~= 0)
if foll_enable ~= 1 then
return true
end
local foll_sysid = FOLL_SYSID:get()
if mnt1_enable then
local mnt1_sysid = Parameter("MNT1_SYSID_DFLT"):get()
if mnt1_sysid > 0 and foll_sysid ~= mnt1_sysid then
return false
end
end
if mnt2_enable then
local mnt2_sysid = Parameter("MNT2_SYSID_DFLT"):get()
if mnt2_sysid > 0 and foll_sysid ~= mnt2_sysid then
return false
end
end
return true
end
-- is the RTL altitude legal? This defaults to 120m (400') as set in ARM_ALT_LEGAL
local function rtl_altitude_legal()
-- plane uses RTL_ALTITUDE in meters, copter uses RTL_ALT_M also in meters
-- RTL_ALTITUDE will be correct for either plane or copter
if (RTL_ALTITUDE ~= nil and (RTL_ALTITUDE:get()) > alt_legal_max) then
return false
end
return true
end
-- is the Q RTL altitude (quadplane) legal? This defaults to 120m (400') as set in ARM_ALT_LEGAL
local function q_rtl_alt_legal()
-- Plane specific Parameters
if Q_ENABLE:get() ~= 1 then
return true
end
-- QuadPlane specific Paraemters
local q_rtl_alt = Parameter("Q_RTL_ALT"):get()
if q_rtl_alt > alt_legal_max then
return false
end
return true
end
-- is the RTL climb minimum legal? This defaults to 120m (400')
local function rtl_climb_legal()
if (RTL_CLIMB_MIN ~= nil) and (RTL_CLIMB_MIN:get() > alt_legal_max) then
return false
end
return true
end
-- Display a message as to whether failsafe will QLand or QRTL
local function qland_warning()
if Q_ENABLE:get() ~= 1 then
return true
end
-- Only applies to QuadPlane
local q_options = Parameter("Q_OPTIONS"):get()
local q_land_option = ((q_options & (1 << 5)) > 0)
local q_RTL_option = ((q_options & (1 << 20)) > 0)
if (not q_land_option) and (not q_RTL_option) then
return false
end
return true -- always ok if not a QuadPlane
end
local function qrtl_warning()
if Q_ENABLE:get() ~= 1 then
return true -- always ok if not a QuadPlane
end
local q_options = Parameter("Q_OPTIONS"):get()
local q_land_option = ((q_options & (1 << 5)) > 0)
local q_RTL_option = ((q_options & (1 << 20)) > 0)
if q_land_option or q_RTL_option then
return false
end
return true
end
-- A single check to ensure AIRSPEED_STALL < AIRSPEED_MIN < AIRSPEED_CRUISE < AIRSPEED_MAX
local function airspeed_check()
local airspeed_stall = AIRSPEED_STALL:get()
local airspeed_min = AIRSPEED_MIN:get()
local airspeed_cruise = AIRSPEED_CRUISE:get()
local airspeed_max = AIRSPEED_MAX:get()
if airspeed_stall > 0 then
if airspeed_min < airspeed_stall then
return false
end
end
if airspeed_cruise < airspeed_min then
return false
end
if airspeed_max < airspeed_cruise then
return false
end
return true
end
-- If AIRSPEED_STALL is set AIRSPEED_MIN should be at least 25% higher than AIRSPEED_STALL
local function stallspeed_check()
local airspeed_stall = AIRSPEED_STALL:get()
local airspeed_min = AIRSPEED_MIN:get()
if airspeed_stall > 0 then
if airspeed_min > (airspeed_stall * 1.25) then
return false
end
end
return true
end
-- From the wiki: This is the center of the speed scaling range and should be set close to the normal cruising speed of the vehicle.
local function scaling_speed_check()
local scaling_speed = SCALING_SPEED:get()
local airspeed_cruise = AIRSPEED_CRUISE:get()
if scaling_speed <= 0 or airspeed_cruise <= 0 then
return false
end
-- use a 20% range for "close"
if scaling_speed < (airspeed_cruise * 0.8) or scaling_speed > (airspeed_cruise * 1.2) then
return false
end
-- if either are not set then
return true
end
-- want to be notified if motors are emergency stopped or not
local function motors_emergency_stopped()
return not SRV_Channels:get_emergency_stop()
end
-- Logic provided by Peter Barker
-- Fences present if
-- a. there are basic fences (assume this includes min(8)/max(1) and home circle(2) fences) or
-- b. there are polygon_fences (fence type bit 2 = 4) with at least one side
local function fence_present()
local enabled_fences = FENCE_TYPE:get()
local basic_fence = (enabled_fences & (8+2+1)) ~= 0
local polygon_fence = ((enabled_fences & 4) ~= 0) and FENCE_TOTAL:get() > 0
return basic_fence or polygon_fence
end
-- check if any rally point is too far away aka further than RALLY_LIMIT_KM away from home
local function rally_ok()
if rally == nil then
return true -- rally library not loaded so we pass
end
local home_location = ahrs:get_home()
local rally_distance_max_m = RALLY_LIMIT_KM:get() * 1000.0
if rally_distance_max_m <= 0 then
return true
end
local i = 0
repeat
local rally_location = rally:get_rally_location(i)
if rally_location then
local rally_distance_m = home_location:get_distance(rally_location)
if rally_distance_m > rally_distance_max_m then
return false
end
end
i = i + 1
until rally_location == nil
return true
end
-- fail if there is a fence on the board, it must be enabled
local function geofence_present_enabled()
if not fence_present() then
return true
end
if (FENCE_ENABLE:get() == 1) or (FENCE_AUTOENABLE:get() > 0) then
return true
end
return false
end
-- Arming checks can be deleted if not required, or set the parameter to MAV_SEVERITY.NONE to avoid changing the script
-- or new arming checks added. The position/number of the check should not be changed. If you delete a check, do not renumber the rest.
-- First add checks that can apply to all vehicles.
local arming_checks = {}
--[[
// @Param: ARM_SYSID
// @DisplayName: MAV_SYSID must be set
// @Description: Check that MAV_SYSID (or SYDID_THISMAV) has been set. 3 or less to prevent arming. -1 to disable.
// @Values: -1:Disabled,0:Emergency(PreArm),1:Alert(PreArm),2:Critical(PreArm),3:Error(PreArm),4:Warning,5:Notice,6:Info,7:Debug
// @User: Standard
--]]
table.insert(arming_checks, Arming_Check(FIRMWARE.GLOBAL, 1,
sysid_set, "SYSID", "MAV_SYSID not set", MAV_SEVERITY.WARNING, false, false))
--[[
// @Param: ARM_FOLL_SYSID
// @DisplayName: FOLL_SYSID must be set
// @Description: If FOLL_ENABLE = 1, check that FOLL_SYSID has been set. 3 or less to prevent arming. -1 to disable.
// @Values: -1:Disabled,0:Emergency(PreArm),1:Alert(PreArm),2:Critical(PreArm),3:Error(PreArm),4:Warning,5:Notice,6:Info,7:Debug
// @User: Standard
--]]
table.insert(arming_checks, Arming_Check(FIRMWARE.GLOBAL, 2,
foll_sysid_set, "FOLL_SYSID", "FOLL_SYSID not set", MAV_SEVERITY.ERROR, true, false ))
--[[
// @Param: ARM_FOLL_SYSID_X
// @DisplayName: Vehicle should not follow itself
// @Description: If FOLL_ENABLE = 1, check that FOLL_SYSID is different to MAV_SYSID. 3 or less to prevent arming. -1 to disable.
// @Values: -1:Disabled,0:Emergency(PreArm),1:Alert(PreArm),2:Critical(PreArm),3:Error(PreArm),4:Warning,5:Notice,6:Info,7:Debug
// @User: Standard
--]]
table.insert(arming_checks, Arming_Check(FIRMWARE.GLOBAL, 3,
foll_sysid_not_thismav, "FOLL_SYSID_X", "FOLL_SYSID == MAV_SYSID", MAV_SEVERITY.ERROR, true, false))
--[[
// @Param: ARM_FOLL_OFS_DEF
// @DisplayName: Follow Offsets defaulted
// @Description: Follow offsets should not be left as default (zero) if FOLL_ENABLE = 1. 3 or less to prevent arming. -1 to disable.
// @Values: -1:Disabled,0:Emergency(PreArm),1:Alert(PreArm),2:Critical(PreArm),3:Error(PreArm),4:Warning,5:Notice,6:Info,7:Debug
// @User: Standard
--]]
table.insert(arming_checks, Arming_Check(FIRMWARE.GLOBAL, 4,
foll_ofs_default, "FOLL_OFS_DEF", "FOLL_OFS_[XYZ] = 0", MAV_SEVERITY.ERROR, true, false))
--[[
// @Param: ARM_MNTX_SYSID
// @DisplayName: Follow and Mount should follow the same vehicle
// @Description: If FOLL_ENABLE = 1 and MNTx_SYSID_DEFLT is set, check that FOLL_SYSID is equal MNTx. 3 or less to prevent arming. -1 to disable.
// @Values: -1:Disabled,0:Emergency(PreArm),1:Alert(PreArm),2:Critical(PreArm),3:Error(PreArm),4:Warning,5:Notice,6:Info,7:Debug
// @User: Standard
--]]
table.insert(arming_checks, Arming_Check(FIRMWARE.GLOBAL, 5,
mntx_sysid_match, "MNTX_SYSID", "MNTx_SYSID != FOLL", MAV_SEVERITY.WARNING, true, false))
--[[
// @Param: ARM_RTL_CLIMB
// @DisplayName: RTL_CLIMB_MIN should be a valid value
// @Description: RTL_CLIMB_MIN should be < 120m (400ft). 3 or less to prevent arming. -1 to disable.
// @Values: -1:Disabled,0:Emergency(PreArm),1:Alert(PreArm),2:Critical(PreArm),3:Error(PreArm),4:Warning,5:Notice,6:Info,7:Debug
// @User: Standard
--]]
table.insert(arming_checks, Arming_Check(FIRMWARE.GLOBAL, 6,
rtl_climb_legal, "RTL_CLIMB", "RTL_CLIMB_MIN too high", MAV_SEVERITY.WARNING, true, false))
--[[
// @Param: ARM_ESTOP
// @DisplayName: Motors EStopped
// @Description: Emergency Stop disables arming. 3 or less to prevent arming. -1 to disable.
// @Values: -1:Disabled,0:Emergency(PreArm),1:Alert(PreArm),2:Critical(PreArm),3:Error(PreArm),4:Warning,5:Notice,6:Info,7:Debug
// @User: Standard
--]]
table.insert(arming_checks, Arming_Check(FIRMWARE.GLOBAL, 7,
motors_emergency_stopped, "ESTOP", "Motors EStopped", MAV_SEVERITY.ERROR, true, false))
--[[
// @Param: ARM_FENCE
// @DisplayName: Fence not enabled
// @Description: Fences loaded but no fence enabled. 3 or less to prevent arming. -1 to disable.
// @Values: -1:Disabled,0:Emergency(PreArm),1:Alert(PreArm),2:Critical(PreArm),3:Error(PreArm),4:Warning,5:Notice,6:Info,7:Debug
// @User: Standard
--]]
table.insert(arming_checks, Arming_Check(FIRMWARE.GLOBAL, 8,
geofence_present_enabled, "FENCE", "Fence not enabled", MAV_SEVERITY.WARNING, true, false))
--[[
// @Param: ARM_RALLY
// @DisplayName: Rally too far
// @Description: Rally Point more than RALLY_LIMIT_KM kilometers away. 3 or less to prevent arming. -1 to disable.
// @Values: -1:Disabled,0:Emergency(PreArm),1:Alert(PreArm),2:Critical(PreArm),3:Error(PreArm),4:Warning,5:Notice,6:Info,7:Debug
// @User: Standard
--]]
table.insert(arming_checks, Arming_Check(FIRMWARE.GLOBAL, 9,
rally_ok, "RALLY", "Rally too far", MAV_SEVERITY.WARNING, true, false))
-- ArduCopter specific checks. Note that the index number starts from 1 for FIRMWARE.COPTER (ARM_C_ parameters)
if FWVersion:type() == FIRMWARE.COPTER then -- add Copter specific Parameters
--[[
// @Param: ARM_C_RTL_ALT_M
// @DisplayName: RTL_ALT_M should be a valid value
// @Description: RTL_ALT_M should be < 120m (400ft). 3 or less to prevent arming. -1 to disable.
// @Values: -1:Disabled,0:Emergency(PreArm),1:Alert(PreArm),2:Critical(PreArm),3:Error(PreArm),4:Warning,5:Notice,6:Info,7:Debug
// @User: Standard
--]]
table.insert(arming_checks, Arming_Check(FIRMWARE.COPTER, 1,
rtl_altitude_legal, "RTL_ALT", "RTL_ALT_M too high", MAV_SEVERITY.ERROR, false, false))
elseif FWVersion:type() == FIRMWARE.PLANE then -- add Plane specific Parameters
-- ArduPlane specific checks. Note that the index number starts from 1 for FIRMWARE.PLANE (ARM_P_ parameters)
--[[
// @Param: ARM_P_Q_FS_LAND
// @DisplayName: Warn if Q failsafe will land
// @Description: Notify the user that on failsafe a QuadPlan will land. 3 or less to prevent arming. -1 to disable.
// @Values: -1:Disabled,0:Emergency(PreArm),1:Alert(PreArm),2:Critical(PreArm),3:Error(PreArm),4:Warning,5:Notice,6:Info,7:Debug
// @User: Standard
--]]
table.insert(arming_checks, Arming_Check(FIRMWARE.PLANE, 1,
qland_warning, "Q_FS_LAND", "Q will land on failsafe", MAV_SEVERITY.NOTICE, true, false))
--[[
// @Param: ARM_P_Q_FS_RTL
// @DisplayName: Warn if Q failsafe will QRTL
// @Description: Notify the user that on failsafe a QuadPlan will QRTL. 3 or less to prevent arming. -1 to disable.
// @Values: -1:Disabled,0:Emergency(PreArm),1:Alert(PreArm),2:Critical(PreArm),3:Error(PreArm),4:Warning,5:Notice,6:Info,7:Debug
// @User: Standard
--]]
table.insert(arming_checks, Arming_Check(FIRMWARE.PLANE, 2,
qrtl_warning, "Q_FS_RTL", "Q will RTL on failsafe", MAV_SEVERITY.NOTICE, true, false))
--[[
// @Param: ARM_P_AIRSPEED
// @DisplayName: Check AIRSPEED_ parameters
// @Description: Validate that AIRSPEED_STALL(if set) < MIN < CRUISE < MAX d. 3 or less to prevent arming. -1 to disable.
// @Values: -1:Disabled,0:Emergency(PreArm),1:Alert(PreArm),2:Critical(PreArm),3:Error(PreArm),4:Warning,5:Notice,6:Info,7:Debug
// @User: Standard
--]]
table.insert(arming_checks, Arming_Check(FIRMWARE.PLANE, 3,
airspeed_check, "AIRSPEED", "stall < min < crs < max", MAV_SEVERITY.ERROR, true, false))
--[[
// @Param: ARM_P_STALL
// @DisplayName: AIRSPEED_MIN should be 25% above STALL
// @Description: Validate that AIRSPEED_MIN is at least 25% above AIRSPEED_STALL(if set). 3 or less to prevent arming. -1 to disable.
// @Values: -1:Disabled,0:Emergency(PreArm),1:Alert(PreArm),2:Critical(PreArm),3:Error(PreArm),4:Warning,5:Notice,6:Info,7:Debug
// @User: Standard
--]]
table.insert(arming_checks, Arming_Check(FIRMWARE.PLANE, 4,
stallspeed_check, "STALL", "Min speed not 25% above stall", MAV_SEVERITY.INFO, true, false))
--[[
// @Param: ARM_P_SCALING
// @DisplayName: SCALING_SPEED valid
// @Description: Validate that SCALING_SPEED is within 20% of AIRSPEED_CRUISE. If SCALING_SPEED changes the vehicle may need to be retuned. 3 or less to prevent arming. -1 to disable.
// @Values: -1:Disabled,0:Emergency(PreArm),1:Alert(PreArm),2:Critical(PreArm),3:Error(PreArm),4:Warning,5:Notice,6:Info,7:Debug
// @User: Standard
--]]
table.insert(arming_checks, Arming_Check(FIRMWARE.PLANE, 5,
scaling_speed_check, "SCALING", "Scaling spd >< 20% of CRUISE", MAV_SEVERITY.ERROR, true, false))
--[[
// @Param: ARM_P_RTL_ALT
// @DisplayName: RTL_ALTITUDE should be a valid value
// @Description: RTL_ALTITITUDE should be < 120m (400ft). 3 or less to prevent arming. -1 to disable.
// @Values: -1:Disabled,0:Emergency(PreArm),1:Alert(PreArm),2:Critical(PreArm),3:Error(PreArm),4:Warning,5:Notice,6:Info,7:Debug
// @User: Standard
--]]
table.insert(arming_checks, Arming_Check(FIRMWARE.PLANE, 6,
rtl_altitude_legal, "RTL_ALT", "RTL_ALTITUDE too high", MAV_SEVERITY.ERROR, false, false))
--[[
// @Param: ARM_P_QRTL_ALT
// @DisplayName: Q_RTL_ALT should be a valid value
// @Description: Q_RTL_ALT should be < 120m (400ft). 3 or less to prevent arming. -1 to disable.
// @Values: -1:Disabled,0:Emergency(PreArm),1:Alert(PreArm),2:Critical(PreArm),3:Error(PreArm),4:Warning,5:Notice,6:Info,7:Debug
// @User: Standard
--]]
table.insert(arming_checks, Arming_Check(FIRMWARE.PLANE, 7,
q_rtl_alt_legal, "QRTL_ALT", "Q_RTL_ALT too high", MAV_SEVERITY.ERROR, false, false))
end
-- No need to actively check while armed.
local function idle_while_armed()
if not arming:is_armed() then return validate, REFRESH_DELAY end
return idle_while_armed, REFRESH_DELAY * 20
end
-- initialize all the parameters based on the data in the arming_checks table
local function initialize()
-- special case for the ARV_ALT_LEGAL parameter which ideally should be a "regular" parameter
--[[
// @Param: ARM_V_ALT_LEGAL
// @DisplayName: Legal max altitude
// @Description: Legal max altitude for UAV/RPAS/drones in your jurisdiction
// @Units: m
// @User: Standard
--]]
assert(param:add_param(VALUES_PARAM_TABLE_KEY, 1, "ALT_LEGAL", 120.0),
string.format('could not add param %s', VALUES_PARAM_TABLE_PREFIX.."ALT_LEGAL"))
ARM_V_ALT_LEGAL:init("ARM_V_ALT_LEGAL")
for _, check in ipairs(arming_checks) do
local param_name = VEHICLE[check.firmware].prefix..check.param_name
assert(param:add_param(VEHICLE[check.firmware].key , check.index, check.param_name, check.severity),
string.format('could not add param %s', param_name))
end
end
-- run an indivual check
local function run_check(check, severity, validated)
local check_passed = check:state() -- sets check.passed and check.changed
local raise_prearm = not check_passed
if check.changed then
if check_passed then
-- it's passed now, but changed, so previously failed. Let the user know its all clear
gcs:send_text(MAV_SEVERITY.INFO, string.format('%s: clear: %s', SCRIPT_NAME_SHORT, check.text))
elseif severity > MAV_SEVERITY.ERROR then
-- deal with warning messages (MAV_SEVERITY > ERROR), we display a message but no prearm
gcs:send_text(severity, string.format('%s: %s: %s', SCRIPT_NAME_SHORT,
MAV_SEVERITY_TEXT[severity], check.text))
-- display the message, but this should not block arming so is actually "passed"
raise_prearm = false
end
elseif not check_passed and severity > MAV_SEVERITY.ERROR then
-- if nothing changed we only want to fail on errors
raise_prearm = false
end
if raise_prearm then
validated = false
arming:set_aux_auth_failed(arm_auth_id, string.format('%s: %s: %s', SCRIPT_NAME_SHORT,
MAV_SEVERITY_TEXT[severity], check.text))
end
return validated
end
validate = function() -- this is the loop which periodically runs to do the validations
if arming:is_armed() then return idle_while_armed() end
alt_legal_max = ARM_V_ALT_LEGAL:get() or 120.0 -- used Parameter():init() to initialize this one.
local validated = true
for _, check in ipairs(arming_checks) do
local param_name = VEHICLE[check.firmware].prefix..check.param_name
local parameter = Parameter(param_name)
local param_severity = parameter:get()
if param_severity ~= MAV_SEVERITY.NONE then -- ignore if NONE
validated = run_check(check, param_severity, validated)
end
end
if validated then
arming:set_aux_auth_passed(arm_auth_id)
end
return validate, REFRESH_DELAY
end
local function delayed_startup()
gcs:send_text(MAV_SEVERITY.INFO, string.format("%s %s loaded", SCRIPT_NAME, SCRIPT_VERSION) )
return validate, REFRESH_DELAY
end
initialize()
-- start running update loop - waiting 20s for the AP to initialize so we don't spam the user with spurios notices
return delayed_startup, INITIAL_DELAY
Reference in New Issue View Git Blame Copy Permalink