Use Lua for Fence arming checks

Tools/autotest/arduplane.py

@@ -3305,6 +3305,45 @@ def attempt_fence_breached_disable(start_mode, end_mode, expected_mode, action):
         attempt_fence_breached_disable(start_mode="FBWA", end_mode="FBWA", expected_mode="GUIDED", action=6)
         attempt_fence_breached_disable(start_mode="FBWA", end_mode="FBWA", expected_mode="GUIDED", action=7)
 
+    def test_fence_script_applet_arming_checks(self):
+        """ Applet for Arming Checks will prevent a vehicle from enabling a GeoFence
+            When there are no fences present
+            """
+        self .start_subtest("Fence Arming Script Applet validation")
+        self.context_push()
+
+        applet_script = "arming-checks.lua"
+        try:
+            """Initialize the FC"""
+            self.set_parameter("SCR_ENABLE", 1)
+            self.install_applet_script(applet_script)
+            self.reboot_sitl()
+            """Make sure here is no fence - then try to enable the fence"""
+            self.clear_fence()
+            self.wait_ready_to_arm()
+
+            self.context_collect('STATUSTEXT')
+
+            self .start_subsubtest("FENCE_ENABLE")
+            self.set_parameter("FENCE_ENABLE", 1)
+            self.assert_prearm_failure("PreArm: FENCE_ENABLE = 1 but no fence present")
+            self.set_parameter("FENCE_ENABLE", 0)
+
+            """Now try to autoenable - only fires in AUTO or TAKEOFF (use TAKEOFF since we don't have a mission"""
+            self .start_subsubtest("FENCE_AUTOENABLE")
+            self.change_mode('TAKEOFF')
+            self.set_parameter("FENCE_AUTOENABLE", 1)
+            self.assert_prearm_failure("PreArm: FENCE_AUTOENABLE > 0 but no fence present")
+            self.set_parameter("FENCE_AUTOENABLE", 0)
+
+        except Exception as e:
+            self.print_exception_caught(e)
+
+        self.remove_applet_script(applet_script)
+        self.context_stop_collecting('STATUSTEXT')
+
+        self.context_pop()
+
     def run_auxfunc(self,
                     function,
                     level,
@@ -3923,6 +3962,10 @@ def tests(self):
              "Tests Disabling fence while undergoing action caused by breach",
              self.test_fence_disable_under_breach_action),
 
+            ("FenceArmingApplet",
+             "Test applet for arming checks for Fence",
+             self.test_fence_script_applet_arming_checks),
+
             ("ADSB",
              "Test ADSB",
              self.ADSB),

Tools/autotest/common.py

@@ -6935,6 +6935,9 @@ def mh(mav, m):
         return statustext_full
 
     # routines helpful for testing LUA scripting:
+    def script_applet_source_path(self, scriptname):
+        return os.path.join(self.rootdir(), "libraries", "AP_Scripting", "applets", scriptname)
+
     def script_example_source_path(self, scriptname):
         return os.path.join(self.rootdir(), "libraries", "AP_Scripting", "examples", scriptname)
 
@@ -6952,6 +6955,10 @@ def install_script(self, source, scriptname):
         self.progress("Copying (%s) to (%s)" % (source, dest))
         shutil.copy(source, dest)
 
+    def install_applet_script(self, scriptname):
+        source = self.script_applet_source_path(scriptname)
+        self.install_script(source, scriptname)
+
     def install_example_script(self, scriptname):
         source = self.script_example_source_path(scriptname)
         self.install_script(source, scriptname)
@@ -6960,6 +6967,15 @@ def install_test_script(self, scriptname):
         source = self.script_test_source_path(scriptname)
         self.install_script(source, scriptname)
 
+    def remove_applet_script(self, scriptname):
+        dest = self.installed_script_path(scriptname)
+        try:
+            os.unlink(dest)
+        except IOError:
+            pass
+        except OSError:
+            pass
+
     def remove_example_script(self, scriptname):
         dest = self.installed_script_path(scriptname)
         try:

libraries/AP_Scripting/applets/arming-checks.lua

@@ -0,0 +1,218 @@
+-- This script runs custom arming checks for validations that are important
+-- to some, but might need to be different depending on the vehicle or use case
+-- so we don't want to bake them into the firmware. Requires SCR_ENABLE =1 so must 
+-- be a higher end FC in order to be used. (minimum 2M flash and 1M RAM)
+
+-- What does this do? I can't find any docs that explain this
+-- local auth_id = arming:get_aux_auth_id()
+
+-- defining variables seems to use a lot of memory, so commenting out the values not currently being used.
+
+-- This seems like a pretty quick refresh rate, but it doesn't run if armed
+local REFRESH_RATE = 2000
+local SEVERITY_INFO = 1
+local SEVERITY_WARNING = 2
+local SEVERITY_ERROR = 3
+
+--local MAV_SEVERITY_EMERGENCY = 0    --/* System is unusable. This is a "panic" condition. | */
+--local MAV_SEVERITY_ALERT = 1        --/* Action should be taken immediately. Indicates error in non-critical systems. | */
+--local MAV_SEVERITY_CRITICAL = 2     --/* Action must be taken immediately. Indicates failure in a primary system. | */
+local MAV_SEVERITY_ERROR = 3        --/* Indicates an error in secondary/redundant systems. | */
+local MAV_SEVERITY_WARNING = 4      --/* Indicates about a possible future error if this is not resolved within a given timeframe. Example would be a low battery warning. | */
+--local MAV_SEVERITY_NOTICE = 5       --/* An unusual event has occurred, though not an error condition. This should be investigated for the root cause. | */
+local MAV_SEVERITY_INFO = 6         --/* No rmal operational messages. Useful for logging. No action is required for these messages. | */
+local MAV_SEVERITY_DEBUG = 7        --/* Useful non-operational messages that can assist in debugging. These should not occur during normal operation. | */
+
+-- These are the plane modes that autoenable the geofence
+local PLANE_MODE_AUTO = 10
+local PLANE_MODE_TAKEOFF = 13
+
+local RTL_DISABLE = 0
+--local RTL_THEN_DO_LAND_START = 1
+--local RTL_IMMEDIATE_DO_LAND_START = 2
+--local NO_RTL_GO_AROUND = 3
+
+local STATE_UNKNOWN = 0
+local STATE_OK = 1
+local STATE_WARNING = 2
+local STATE_ERROR = 3
+
+-- I think registering for an arming "aux_auth_id" allows the script to block arming
+local arm_auth_id = arming:get_aux_auth_id()
+local armed_old = arming:is_armed()
+local vehicle_type = FWVersion:type()
+local mode = vehicle:get_mode()
+
+local RESOLVED = "Resolved:"
+
+-- It might confuse a pilot to enable GeoFence when there is no fence on the vehicle,
+-- as they might think they they are "protected" when they are actually not
+-- This throws two different errors. One if FENCE_ENABLE is true and one 
+-- if FENCE_AUTOENABLE is set but only when triggering a AUTO takeoff
+local geofence_enable = -1
+local geofence_enable_state = STATE_UNKNOWN
+local geofence_enable_message = "FENCE_ENABLE = 1 but no fence present"
+local geofence_enable_present = -1
+local function validate_geofence_enable(severity)
+    local present_new = AC_Fence:present()
+    if(present_new ~= geofence_enable_present) then
+        geofence_enable_present = present_new
+        geofence_enable = -1
+    end
+    local geofence_enable_new = param:get('FENCE_ENABLE')
+    if geofence_enable == geofence_enable_new then
+        return geofence_enable_state ~= STATE_ERROR
+    end
+    geofence_enable = geofence_enable_new
+    --gcs:send_text(MAV_SEVERITY_DEBUG, "FENCE_ENABLE: "..geofence_enable)
+    if geofence_enable ~= 0 and not geofence_enable_present then
+        --gcs:send_text(MAV_SEVERITY_DEBUG, "FENCE_ENABLE: no fence")
+        if severity == SEVERITY_ERROR then
+            if geofence_enable_state ~= STATE_ERROR then
+                arming:set_aux_auth_failed(arm_auth_id, geofence_enable_message)
+                gcs:send_text(MAV_SEVERITY_ERROR, geofence_enable_message)
+                geofence_enable_state = STATE_ERROR
+            end
+            return false
+        elseif severity == SEVERITY_WARNING or severity == SEVERITY_INFO then
+            if geofence_enable_state ~= STATE_WARNING then
+                gcs:send_text(MAV_SEVERITY_WARNING, geofence_enable_message)
+                geofence_enable_state = STATE_WARNING
+            end
+        end
+    elseif geofence_enable_state ~= STATE_OK then
+        --gcs:send_text(MAV_SEVERITY_DEBUG, "FENCE_ENABLE: old state:" .. geofence_enable_state)
+
+        if(geofence_enable_state ~= STATE_UNKNOWN) then
+            gcs:send_text(MAV_SEVERITY_INFO,RESOLVED..geofence_enable_message)
+        end
+        geofence_enable_state = STATE_OK
+    end
+    return true
+end 
+
+
+local geofence_autoenable = -1
+local geofence_autoenable_state = STATE_UNKNOWN
+local geofence_autoenable_message = "FENCE_AUTOENABLE > 0 but no fence present"
+local geofence_autoenable_present = -1
+local function validate_geofence_autoenable(severity)
+    local mode_new = vehicle:get_mode()
+    if mode_new ~= mode then
+        -- If the mode changed, the results might be different, so need to force recheck
+        mode    = mode_new
+        geofence_autoenable = -1
+    end
+    local present_new = AC_Fence:present()
+    if(present_new ~= geofence_autoenable_present) then
+        geofence_autoenable_present = present_new
+        geofence_autoenable = -1
+    end
+    local geofence_autoenable_new = param:get('FENCE_AUTOENABLE')
+    gcs:send_text(MAV_SEVERITY_DEBUG, "FENCE_autoENABLE: "..geofence_autoenable)
+    if geofence_autoenable_new == geofence_autoenable then
+        return geofence_autoenable_state ~= STATE_ERROR
+    end
+    geofence_autoenable = geofence_autoenable_new
+
+        -- we only want to warn the user if the mode is currently AUTO or TAKEOFF
+        -- otherwise we let the AP code warn the user when/if the fence is actually enabled
+        -- These are the plane checks (type == 3) - the modes are DIFFERENT depending on the vehicle
+    --gcs:send_text(MAV_SEVERITY_DEBUG, "AUTOENABLE vehicle:"..vehicle_type.." mode:"..mode )
+    if geofence_autoenable > 0 and
+        (vehicle_type == 3 and (mode == PLANE_MODE_AUTO or mode == PLANE_MODE_TAKEOFF)) and
+        not geofence_autoenable_present then
+        if severity == SEVERITY_ERROR then
+            if geofence_autoenable_state ~= STATE_ERROR then
+                arming:set_aux_auth_failed(arm_auth_id, geofence_autoenable_message)
+                gcs:send_text(MAV_SEVERITY_ERROR, geofence_autoenable_message)
+                geofence_autoenable_state = STATE_ERROR
+            end
+            return false
+        else 
+            if geofence_autoenable_state ~= SEVERITY_WARNING then
+                gcs:send_text(MAV_SEVERITY_WARNING, geofence_autoenable_message)
+                geofence_autoenable_state = STATE_WARNING
+            end
+        end
+    elseif geofence_autoenable_state ~= STATE_OK then
+        if(geofence_autoenable_state ~= STATE_UNKNOWN) then
+            gcs:send_text(MAV_SEVERITY_INFO,RESOLVED..geofence_autoenable_message)
+        end
+        geofence_autoenable_state = STATE_OK
+    end
+
+    return true
+end
+
+-- This was added to arduplane in PR#20345. Adding this here
+-- makes it possible for this check to be optional
+local rtlautoland_old = -1
+local rtlautoland_state = STATE_UNKNOWN
+local rtlautoland_message = "DO_LAND_START set and RTL_AUTOLAND disabled"
+local function validate_rtlautoland(severity)
+    local rtlautoland = param:get('RTL_AUTOLAND')
+    if rtlautoland_old == rtlautoland then
+        return rtlautoland_state ~= STATE_ERROR
+    end
+    rtlautoland_old = rtlautoland
+    if rtlautoland == RTL_DISABLE then
+        --gcs:send_text(MAV_SEVERITY_DEBUG,"RTL Disabled")
+        local landing_start_count = mission:get_landing_sequence_start()
+        if landing_start_count > 0 then
+            --gcs:send_text(MAV_SEVERITY_DEBUG,"landing_start count > 0")
+            if severity == SEVERITY_ERROR then
+                if rtlautoland_state ~= STATE_ERROR then -- don't spam the GCS - only send the message once
+                    arming:set_aux_auth_failed(arm_auth_id, rtlautoland_message)
+                    gcs:send_text(MAV_SEVERITY_ERROR, rtlautoland_message)
+                    rtlautoland_state = STATE_ERROR
+                --else
+                    --gcs:send_text(MAV_SEVERITY_DEBUG,rtlautoland_message)
+                end
+                return false
+            elseif severity == SEVERITY_WARNING then
+                if rtlautoland_state ~=STATE_WARNING then -- Don't spam the GCS - only send the message once
+                    gcs:send_text(MAV_SEVERITY_WARNING, rtlautoland_message)
+                    rtlautoland_state = STATE_WARNING
+                end
+            end
+        end
+    elseif rtlautoland_state ~= STATE_OK then
+        if(rtlautoland_state ~= STATE_UNKNOWN) then
+            gcs:send_text(MAV_SEVERITY_INFO,RESOLVED..rtlautoland_message)
+        end
+        rtlautoland_state = STATE_OK
+    end
+    return true
+end
+
+function validate() -- this is the loop which periodically runs 
+
+    -- (I think?) We only want to run these checks if we are disarmed
+    if  arming:is_armed() then
+        return validate(), REFRESH_RATE
+    end
+
+    -- Run all the checks we want - you can edit this to decide if you want these checks or not
+    -- each check takes a "Severity" parameter to decide if it's a warning or an error
+   local validated = true
+    validated = validated and validate_geofence_enable(SEVERITY_ERROR)
+    validated = validated and validate_geofence_autoenable(SEVERITY_ERROR)
+    validated = validated and validate_rtlautoland(SEVERITY_WARNING)
+
+    if validated then
+        arming:set_aux_auth_passed(arm_auth_id)  
+    end
+
+    -- do some garbage collection before returning
+    -- local memory_count = collectgarbage("count")
+    -- gcs:send_text(MAV_SEVERITY_DEBUG,"memory:"..memory_count)
+
+    return validate, REFRESH_RATE 
+end
+
+gcs:send_text(MAV_SEVERITY_INFO, "ARMING: scripted validation active")
+
+return validate() -- run immediately before starting to reschedule
+
+

libraries/AP_Scripting/applets/arming-checks.md

@@ -0,0 +1,19 @@
+# arming-checks.lua
+
+NOTE: probably requires increasing SCR_HEAP_SIZE to at least 204800
+
+This script executes pre-arm checks on a flight controller. 
+Checks can be flagged as SEVERITY_ERROR which will prevent arming or
+SEVERITY_WARNING which will display a warning message on the GCS
+
+Multiple checks can be added or removed.
+
+The code has been written to avoid spamming the GCS, so in most cases, the user will be
+notified only if the error/warning condition trips (failes) and again if the issue is resolved.
+
+To add new checks, copy one of the existing methods to reuse this pattern to avoid spam messages.
+
+The default checks are:
+GeoFence enabled when there is no fence set up on the flight controler
+GeoFence autoenable set when there is no fence - but only triggers in AUTO or TAKEOFF mode
+Tridge's RTL_AUTOLAND check, which can now be disabled (or reverted to a warning)

libraries/AP_Scripting/generator/description/bindings.desc

@@ -1,4 +1,4 @@
--- Location stuff (this is a commented line)
+ -- Location stuff (this is a commented line)
 
 include AP_Common/Location.h
 
@@ -347,6 +347,7 @@ singleton AP_Mission method num_commands uint16_t
 singleton AP_Mission method get_item boolean uint16_t 0 UINT16_MAX mavlink_mission_item_int_t'Null
 singleton AP_Mission method set_item boolean uint16_t 0 UINT16_MAX mavlink_mission_item_int_t
 singleton AP_Mission method clear boolean
+singleton AP_Mission method get_landing_sequence_start uint16_t
 
 userdata mavlink_mission_item_int_t field param1 float'skip_check read write
 userdata mavlink_mission_item_int_t field param2 float'skip_check read write
@@ -526,6 +527,9 @@ singleton AP_Mount method set_angle_target void uint8_t 0 UINT8_MAX float'skip_c
 singleton AP_Mount method set_rate_target void uint8_t 0 UINT8_MAX float'skip_check float'skip_check float'skip_check boolean
 singleton AP_Mount method set_roi_target void uint8_t 0 UINT8_MAX Location
 
+include AC_Fence/AC_Fence.h  depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI
+singleton AC_Fence method present boolean
+
 singleton AP_Logger rename logger
 singleton AP_Logger manual write AP_Logger_Write
 
@@ -554,4 +558,3 @@ userdata uint32_t manual_operator __bnot uint32_t___bnot
 userdata uint32_t manual_operator __tostring uint32_t___tostring
 userdata uint32_t manual toint uint32_t_toint
 userdata uint32_t manual tofloat uint32_t_tofloat
-