diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index dd044e36a0e99e..517b12c8b3f677 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -10729,6 +10729,50 @@ def MAV_CMD_NAV_TAKEOFF_command_int(self): self.reboot_sitl() # unlock home position + def LuaParamSet(self): + '''test param-set.lua applet''' + self.install_applet_script_context("param-set.lua") + self.set_parameters({ + 'SCR_ENABLE': 1, + }) + self.reboot_sitl() + + self.wait_ready_to_arm() # scripts will be ready by now! + self.start_subtest("set RTL_ALT freely") + self.set_parameter("RTL_ALT", 23) + self.set_parameter("RTL_ALT", 28) + + self.start_subtest("Unable to set DISARM_DELAY freely") + self.context_push() + self.context_collect('STATUSTEXT') + old_disarm_delay_value = self.get_parameter('DISARM_DELAY') + self.send_set_parameter_direct('DISARM_DELAY', 78) + self.wait_statustext('param-set: param set denied (DISARM_DELAY)', check_context=True) + self.assert_parameter_value('DISARM_DELAY', old_disarm_delay_value) + self.context_pop() + + self.start_subtest("Disabling applet via parameter should allow freely setting DISARM_DELAY") + self.set_parameter("PARAM_SET_ENABLE", 0) + self.set_parameter("DISARM_DELAY", 56) + + self.start_subtest("Re-enabling applet via parameter should stop freely setting DISARM_DELAY") + self.context_push() + self.context_collect('STATUSTEXT') + self.set_parameter("PARAM_SET_ENABLE", 1) + old_disarm_delay_value = self.get_parameter('DISARM_DELAY') + self.send_set_parameter_direct('DISARM_DELAY', 78) + self.wait_statustext('param-set: param set denied (DISARM_DELAY)', check_context=True) + self.assert_parameter_value('DISARM_DELAY', old_disarm_delay_value) + self.context_pop() + + self.start_subtest("Ensure that parameter values are persistent") + self.set_parameter('DISARM_DELAY', 111) + self.reboot_sitl() + self.assert_parameter_value('DISARM_DELAY', 111) + + # very bad things happen if we don't turn things off at the end.. + self.set_parameter("PARAM_SET_ENABLE", 0) + def tests2b(self): # this block currently around 9.5mins here '''return list of all tests''' ret = ([ @@ -10737,6 +10781,7 @@ def tests2b(self): # this block currently around 9.5mins here self.PositionWhenGPSIsZero, Test(self.DynamicRpmNotches, attempts=4), self.PIDNotches, + self.LuaParamSet, self.RefindGPS, Test(self.GyroFFT, attempts=1, speedup=8), Test(self.GyroFFTHarmonic, attempts=4, speedup=8), diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Callisto-2.4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Callisto-2.4/hwdef.dat index d3d816256bea61..90bd672a82867f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Callisto-2.4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Callisto-2.4/hwdef.dat @@ -1,3 +1,3 @@ include ../Callisto/hwdef.inc -define AP_CUSTOM_FIRMWARE_STRING "ArduCopter V4.5.7-C2.4r7" +define AP_CUSTOM_FIRMWARE_STRING "ArduCopter V4.5.7-C2.4r8" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Callisto-2.5/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Callisto-2.5/hwdef.dat index 56f35f00fa1474..e1aaa91400d1c6 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Callisto-2.5/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Callisto-2.5/hwdef.dat @@ -1,3 +1,3 @@ include ../Callisto/hwdef.inc -define AP_CUSTOM_FIRMWARE_STRING "ArduCopter V4.5.7-C2.5r7" +define AP_CUSTOM_FIRMWARE_STRING "ArduCopter V4.5.7-C2.5r8" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Callisto/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/Callisto/defaults.parm index 332746f7a1caaa..50afecebc7bd35 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Callisto/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/Callisto/defaults.parm @@ -230,7 +230,7 @@ PSC_VELZ_P,2.5 RALLY_INCL_HOME,1 RALLY_LIMIT_KM,0.3 RC_OVERRIDE_TIME,3 -RC_PROTOCOLS,3 +RC_PROTOCOLS,8 RC_SPEED,490 RC1_DZ,50 RC2_DZ,50 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Callisto/hwdef.inc b/libraries/AP_HAL_ChibiOS/hwdef/Callisto/hwdef.inc index 644f457a05b2bd..c4249ed3180d8c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Callisto/hwdef.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/Callisto/hwdef.inc @@ -358,6 +358,47 @@ define MODE_THROW_ENABLED 0 define MODE_TURTLE_ENABLED 0 define MODE_ZIGZAG_ENABLED 0 +define AP_AIRSPEED_ENABLED 0 + +define AP_BARO_BACKEND_DEFAULT_ENABLED 0 +define AP_BARO_MS56XX_ENABLED 1 + +define HAL_DISPLAY_ENABLED 0 + +define HAL_EXTERNAL_AHRS_ENABLED 0 + +define AP_GPS_BACKEND_DEFAULT_ENABLED 0 +define AP_GPS_UBLOX_ENABLED 1 + +define HAL_HOTT_TELEM_ENABLED 0 + +define AP_MAVLINK_FAILURE_CREATION_ENABLED 0 + +define AP_INERTIALSENSOR_KILL_IMU_ENABLED 0 + +define AP_LTM_TELEM_ENABLED 0 + +define AP_MOTORS_FRAME_DEFAULT_ENABLED 0 +define AP_MOTORS_FRAME_OCTAQUAD_ENABLED 1 + +define HAL_MSP_ENABLED 0 + +define AP_NOTIFY_MAVLINK_LED_CONTROL_SUPPORT_ENABLED 0 +define AP_NOTIFY_MAVLINK_PLAY_TUNE_SUPPORT_ENABLED 0 + +define AP_NOTIFY_NCP5623_ENABLED 0 +define AP_NOTIFY_PROFILED_ENABLED 0 + +define HAL_RUNCAM_ENABLED 0 + +define EK3_FEATURE_EXTERNAL_NAV 0 + +define MODE_FLIP_ENABLED 0 +define MODE_FLOWHOLD_ENABLED 0 + +define HAL_PERIPH_ENABLE_SERIAL_OPTIONS +define AP_NETWORKING_BACKEND_PPP 1 + USE_BOOTLOADER_FROM_BOARD Durandal ROMFS scripts/LedFlash.lua libraries/AP_HAL_ChibiOS/hwdef/Callisto/scripts/LedFlash.lua diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index eb155ef7c5d721..af62872fd94be2 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -1443,6 +1443,32 @@ bool AP_Param::is_read_only(void) const return false; } +// returns true if this parameter should be settable via the +// MAVLink interface: +bool AP_Param::allow_set_via_mavlink(uint16_t flags) const +{ + if (is_read_only()) { + return false; + } + + if (flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) { + // the user can set BRD_OPTIONS to enable set of internal + // parameters, for developer testing or unusual use cases + if (!AP_BoardConfig::allow_set_internal_parameters()) { + return false; + } + } + +#if HAL_GCS_ENABLED + // check the MAVLink library is OK with the concept: + if (!gcs().get_allow_param_set()) { + return false; + } +#endif // HAL_GCS_ENABLED + + return true; +} + // set a AP_Param variable to a specified value void AP_Param::set_value(enum ap_var_type type, void *ptr, float value) { diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index 5e65b01fa629d8..ea11514a22b689 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -548,7 +548,11 @@ class AP_Param // return the persistent top level key for the ParamToken key static uint16_t get_persistent_key(uint16_t key) { return var_info(key).key; } - + + // returns true if this parameter should be settable via the + // MAVLink interface: + bool allow_set_via_mavlink(uint16_t flags) const; + // count of parameters in tree static uint16_t count_parameters(void); diff --git a/libraries/AP_Scripting/applets/param-set.lua b/libraries/AP_Scripting/applets/param-set.lua new file mode 100644 index 00000000000000..8400497f21ff43 --- /dev/null +++ b/libraries/AP_Scripting/applets/param-set.lua @@ -0,0 +1,167 @@ +-- Inspect parameter sets received via MAVLink, determine action based +-- on whitelist. + +-- When this script runs and ENABLE is true ArduPilot will stop +-- processing parameter-sets via the GCS library. Instead, this +-- script becomes responsible for setting parameters, and it will +-- only set parameters which are whitelisted. Setting ENABLE to +-- false will allow ArduPilot to set parameters normally. + +-- Setting SCR_ENABLE to false while this script is running in the +-- ENABLE state is... not advised. + +-- How To Use +-- 1. copy this script to the autopilot's "scripts" directory +-- 2. set SCR_ENABLE to 1 + +-- global definitions +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} +local UPDATE_INTERVAL_MS = 10 -- update at about 100hz + +-- prefix for all text messages: +local TEXT_PREFIX_STR = "param-set" + +-- +-- parameter setup +-- +local PARAM_TABLE_KEY = 92 +local PARAM_TABLE_PREFIX = "PARAM_SET_" +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 7), 'could not add param table') + +-- add a parameter and bind it to a variable +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', PARAM_TABLE_PREFIX .. name)) + return Parameter(PARAM_TABLE_PREFIX .. name) +end + +--[[ + // @Param: PARAM_SET_ENABLE + // @DisplayName: Param Set enable + // @Description: Param Set enable + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +local PARAM_SET_ENABLE = bind_add_param("ENABLE", 1, 1) + +-- initialize MAVLink rx with buffer depth and number of rx message IDs to register +mavlink:init(5, 1) + +-- register message id to receive +local PARAM_SET_ID = 23 +mavlink:register_rx_msgid(PARAM_SET_ID) + +-- handle PARAM_SET message +local parameters_which_can_be_set = {} +parameters_which_can_be_set["MAV_OPTIONS"] = true +parameters_which_can_be_set["PARAM_SET_ENABLE"] = true +parameters_which_can_be_set["BATT_ARM_MAH"] = true +parameters_which_can_be_set["BATT_ARM_VOLT"] = true +parameters_which_can_be_set["BATT_CAPACITY"] = true +parameters_which_can_be_set["BATT_CRT_MAH"] = true +parameters_which_can_be_set["BATT_CRT_VOLT"] = true +parameters_which_can_be_set["BATT_FS_CRT_ACT"] = true +parameters_which_can_be_set["BATT_FS_LOW_ACT"] = true +parameters_which_can_be_set["BATT_LOW_MAH"] = true +parameters_which_can_be_set["BATT_LOW_VOLT"] = true +parameters_which_can_be_set["BRD_OPTIONS"] = true +parameters_which_can_be_set["COMPASS_USE3"] = true +parameters_which_can_be_set["FENCE_ACTION"] = true +parameters_which_can_be_set["FENCE_ALT_MAX"] = true +parameters_which_can_be_set["FENCE_ENABLE"] = true +parameters_which_can_be_set["FENCE_RADIUS"] = true +parameters_which_can_be_set["FENCE_TYPE"] = true +parameters_which_can_be_set["LIGHTS_ON"] = true +parameters_which_can_be_set["LOG_BITMASK"] = true +parameters_which_can_be_set["LOG_DISARMED"] = true +parameters_which_can_be_set["LOG_FILE_DSRMROT"] = true +parameters_which_can_be_set["RTL_ALT"] = true +parameters_which_can_be_set["RTL_LOIT_TIME"] = true +parameters_which_can_be_set["RTL_SPEED"] = true + +local function should_set_parameter_id(param_id) + if parameters_which_can_be_set[param_id] == nil then + return false + end + return parameters_which_can_be_set[param_id] +end + +local function handle_param_set(name, value) + -- we will not receive packets in here for the wrong system ID / + -- component ID; this is handled by ArduPilot's MAVLink routing + -- code + + -- check for this specific ID: + if not should_set_parameter_id(name) then + gcs:send_text(MAV_SEVERITY.WARNING, string.format("%s: param set denied (%s)", TEXT_PREFIX_STR, name)) + return + end + + param:set_and_save(name, value) + gcs:send_text(MAV_SEVERITY.WARNING, string.format("%s: param set applied", TEXT_PREFIX_STR)) +end + +-- display welcome message +gcs:send_text(MAV_SEVERITY.INFO, "param-set script loaded") + +-- initialise our knowledge of the GCS's allow-set-parameters state. +-- We do not want to fight over setting this GCS state via other +-- mechanisms (eg. an auxiliary function), so we keep this state +-- around to track what we last set: +local gcs_allow_set = gcs:get_allow_param_set() + +-- update function to receive param_set messages and perhaps act on them +local function update() + -- return immediately if not enabled + if (PARAM_SET_ENABLE:get() <= 0) then + -- this script is disabled, set allow-via-GCS (once): + if not gcs_allow_set then + gcs:set_allow_param_set(true) + gcs_allow_set = true + end + -- drain all mavlink messages to avoid processing them when enabled + while true do + local msg, _ = mavlink:receive_chan() + if msg == nil then + break + end + end + return + end + + -- this script is enabled, disallow setting via normal means (once): + if gcs_allow_set then + gcs:set_allow_param_set(false) + gcs_allow_set = false + end + + -- consume all available mavlink messages + while true do + local msg, _ = mavlink:receive_chan() + if msg == nil then + break + end + + local param_value, _, _, param_id, _ = string.unpack("cast_to_float(var_type); - if (parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) { - // the user can set BRD_OPTIONS to enable set of internal - // parameters, for developer testing or unusual use cases - if (AP_BoardConfig::allow_set_internal_parameters()) { - parameter_flags &= ~AP_PARAM_FLAG_INTERNAL_USE_ONLY; + if (!vp->allow_set_via_mavlink(parameter_flags)) { + // don't warn the user about this failure if we are dropping + // messages here. This is on the assumption that scripting is + // currently responsible for setting parameters and may set + // the value instead of us. + if (gcs().get_allow_param_set()) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Param write denied (%s)", key); } - } - - if ((parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) || vp->is_read_only()) { - gcs().send_text(MAV_SEVERITY_WARNING, "Param write denied (%s)", key); // send the readonly value send_parameter_value(key, var_type, old_value); return;