diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 3399e018ba5d3..0031537bb4708 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -1077,11 +1077,11 @@ def should_fetch_all_for_parameter_change(param_name): return True return False - def set_parameter(self, name, value, add_to_context=True, epsilon=0.0002): + def set_parameter(self, name, value, add_to_context=True, epsilon=0.0002, retries=10): """Set parameters from vehicle.""" self.progress("Setting %s to %f" % (name, value)) old_value = self.get_parameter(name, retry=2) - for i in range(1, 10): + for i in range(1, retries+2): self.mavproxy.send("param set %s %s\n" % (name, str(value))) returned_value = self.get_parameter(name) delta = float(value) - returned_value @@ -3070,6 +3070,20 @@ def run_tests(self, tests): return True + def test_parameters(self): + '''general small tests for parameter system''' + self.start_subtest("Ensure GCS is not be able to set MIS_TOTAL") + old_mt = self.get_parameter("MIS_TOTAL") + ex = None + try: + self.set_parameter("MIS_TOTAL", 17, retries=0) + except ValueError as e: + ex = e + if ex is None: + raise NotAchievedException("Set parameter when I shouldn't have") + if old_mt != self.get_parameter("MIS_TOTAL"): + raise NotAchievedException("Total has changed") + def disabled_tests(self): return {} @@ -3126,6 +3140,10 @@ def tests(self): ("SensorConfigErrorLoop", "Test Sensor Config Error Loop", self.test_sensor_config_error_loop), + + ("Parameters", + "Test Parameter Set/Get", + self.test_parameters), ] def post_tests_announcements(self): diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index ba4206a00b205..d32b145230053 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -14,7 +14,8 @@ const AP_Param::GroupInfo AP_Mission::var_info[] = { // @Range: 0 32766 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("TOTAL", 0, AP_Mission, _cmd_total, 0), + // @ReadOnly: True + AP_GROUPINFO_FLAGS("TOTAL", 0, AP_Mission, _cmd_total, 0, AP_PARAM_FLAG_INTERNAL_USE_ONLY), // @Param: RESTART // @DisplayName: Mission Restart when entering Auto mode diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 3ae155c8b4814..e5e97d2f112d3 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -825,7 +825,7 @@ AP_Param::find_group(const char *name, uint16_t vindex, ptrdiff_t group_offset, // Find a variable by name. // AP_Param * -AP_Param::find(const char *name, enum ap_var_type *ptype) +AP_Param::find(const char *name, enum ap_var_type *ptype, uint16_t *flags) { for (uint16_t i=0; i<_num_vars; i++) { uint8_t type = _var_info[i].type; @@ -840,6 +840,9 @@ AP_Param::find(const char *name, enum ap_var_type *ptype) } AP_Param *ap = find_group(name + len, i, 0, group_info, ptype); if (ap != nullptr) { + if (flags != nullptr) { + *flags = group_info->flags; + } return ap; } // we continue looking as we want to allow top level diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index 0efa595113d0f..67ae8d5f76dfb 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -63,13 +63,18 @@ // the var_info is a pointer, allowing for dynamic definition of the var_info tree #define AP_PARAM_FLAG_INFO_POINTER (1<<4) +// this parameter is visible to GCS via mavlink but should never be +// set by anything other than the ArduPilot code responsible for its +// use. +#define AP_PARAM_FLAG_INTERNAL_USE_ONLY (1<<5) + // keep all flags before the FRAME tags // vehicle and frame type flags, used to hide parameters when not // relevent to a vehicle type. Use AP_Param::set_frame_type_flags() to // enable parameters flagged in this way. frame type flags are stored // in flags field, shifted by AP_PARAM_FRAME_TYPE_SHIFT. -#define AP_PARAM_FRAME_TYPE_SHIFT 5 +#define AP_PARAM_FRAME_TYPE_SHIFT 6 // supported frame types for parameters #define AP_PARAM_FRAME_COPTER (1<<0) @@ -247,10 +252,11 @@ class AP_Param /// If the variable has no name, it cannot be found by this interface. /// /// @param name The full name of the variable to be found. + /// @param flags If non-null will be filled with parameter flags /// @return A pointer to the variable, or nullptr if /// it does not exist. /// - static AP_Param * find(const char *name, enum ap_var_type *ptype); + static AP_Param * find(const char *name, enum ap_var_type *ptype, uint16_t *flags = nullptr); /// set a default value by name /// diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index c352916e9e8bc..d10449d414e5b 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -116,6 +116,10 @@ class GCS_MAVLINK virtual uint8_t sysid_my_gcs() const = 0; virtual bool sysid_enforce() const { return false; } + void send_parameter_value(const char *param_name, + ap_var_type param_type, + float param_value); + // NOTE! The streams enum below and the // set of AP_Int16 stream rates _must_ be // kept in the same order diff --git a/libraries/GCS_MAVLink/GCS_Param.cpp b/libraries/GCS_MAVLink/GCS_Param.cpp index 5cd7824f1099c..7ae2fba9615a4 100644 --- a/libraries/GCS_MAVLink/GCS_Param.cpp +++ b/libraries/GCS_MAVLink/GCS_Param.cpp @@ -268,12 +268,24 @@ void GCS_MAVLINK::handle_param_set(const mavlink_message_t &msg) key[AP_MAX_NAME_SIZE] = 0; // find existing param so we can get the old value - vp = AP_Param::find(key, &var_type); + uint16_t parameter_flags = 0; + vp = AP_Param::find(key, &var_type, ¶meter_flags); if (vp == nullptr) { return; } + float old_value = vp->cast_to_float(var_type); + if (parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) { + gcs().send_text(MAV_SEVERITY_WARNING, "Param write denied (%s)", key); + // echo back the incorrect value so that we fulfull the + // parameter state machine requirements: + send_parameter_value(key, var_type, packet.param_value); + // and then announce what the correct value is: + send_parameter_value(key, var_type, old_value); + return; + } + // set the value vp->set_float(packet.param_value, var_type); @@ -295,6 +307,20 @@ void GCS_MAVLINK::handle_param_set(const mavlink_message_t &msg) } } +void GCS_MAVLINK::send_parameter_value(const char *param_name, ap_var_type param_type, float param_value) +{ + if (!HAVE_PAYLOAD_SPACE(chan, PARAM_VALUE)) { + return; + } + mavlink_msg_param_value_send( + chan, + param_name, + param_value, + mav_param_type(param_type), + AP_Param::count_parameters(), + -1); +} + /* send a parameter value message to all active MAVLink connections */