AP_SCripting: POC using Lua for arming checks

Tools/autotest/arduplane.py

@@ -3305,6 +3305,46 @@ 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()
+        ex = None
+
+        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_ekf_happy()
+
+            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:
+            ex = e
+            self.print_exception_caught(e)
+
+        self.remove_applet_script(applet_script)
+        self.context_pop()
+
+        if ex is not None:
+            raise ex
+
     def run_auxfunc(self,
                     function,
                     level,
@@ -3923,6 +3963,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,106 @@
+-- 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)
+-- Thanks to @yuri_rage and Peter Barker for help with the Lua and Autotests
+
+local REFRESH_RATE      = 200
+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_INFO = 6         --/* No rmal operational messages. Useful for logging. No action is required for these messages. | */
+
+-- These are the plane modes that autoenable the geofence
+local PLANE_MODE_AUTO = 10
+local PLANE_MODE_TAKEOFF = 13
+
+local arm_auth_id = arming:get_aux_auth_id()
+
+----  CLASS: Arming_Check  ----
+local Arming_Check = {}
+Arming_Check.__index = Arming_Check
+
+setmetatable(Arming_Check, {
+    __call = function(cls, func, pass_value, severity, text) -- constructor
+        local self      = setmetatable({}, cls)
+        self.func       = func
+        self.pass_value = pass_value
+        self.severity   = severity
+        self.text       = text
+        self.passed     = false
+        self.changed    = false
+        return self
+    end
+})
+
+local function geofence_enabled_armingcheck()
+    --We fail if there is a no fence but FENCE_ENABLE is set - so we pass if NOT that
+    return not (not AC_Fence:present() and param:get('FENCE_ENABLE') == 1)
+end
+local function geofence_autoenabled_armingcheck()
+    --We fail if there is a no fence but FENCE_AUTOENABLE is set - so we pass if NOT that
+    --Plus we only fail this if in AUTO mode or TAKEOFF mode (on a plane)
+    local vehicle_type = FWVersion:type()
+    local mode = vehicle:get_mode()
+
+    if (vehicle_type == 3 and (mode == PLANE_MODE_AUTO or mode == PLANE_MODE_TAKEOFF)) then
+        return not (not AC_Fence:present() and param:get('FENCE_AUTOENABLE') == 1)
+    end
+    -- If this check is useful for other vehicles they will need to add them later
+    return true
+end
+
+function Arming_Check:state()
+    local passed = self.func() == self.pass_value
+    self.changed = false
+    if self.passed ~= passed then
+        self.passed = passed
+        self.changed = true
+    end
+    return self.passed
+end
+
+local arming_checks = {
+    GeoFence_Enabled = Arming_Check(geofence_enabled_armingcheck, 
+                            true, MAV_SEVERITY_ERROR,
+                            "FENCE_ENABLE = 1 but no fence present"),
+    GeoFence_AutoEnabled = Arming_Check(geofence_autoenabled_armingcheck, true, MAV_SEVERITY_ERROR,
+                            "FENCE_AUTOENABLE > 0 but no fence present"
+                            )
+}
+
+local function idle_while_armed()
+    if not arming:is_armed() then return Validate, REFRESH_RATE end
+    return idle_while_armed, REFRESH_RATE * 10
+end
+
+function Validate() -- this is the loop which periodically runs
+
+    if arming:is_armed() then return idle_while_armed() end
+
+    local validated = true
+
+    for key, check in pairs(arming_checks) do
+        validated = validated and check:state()
+        if check.changed  and check.passed then
+            gcs:send_text(MAV_SEVERITY_INFO, string.format('ARMING: %s passed', check.text))
+        end
+        if check.changed  and not check.passed then
+            if check.severity == MAV_SEVERITY_ERROR then
+                gcs:send_text(MAV_SEVERITY_ERROR, string.format('ARMING: %s failed', check.text))
+                arming:set_aux_auth_failed(arm_auth_id, string.format('%s failed', check.text))
+            elseif check.severity == MAV_SEVERITY_WARNING then
+                gcs:send_text(MAV_SEVERITY_WARNING, check.text)
+            end
+        end
+    end
+
+    if validated then
+        arming:set_aux_auth_passed(arm_auth_id)
+    end
+
+    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
-