Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Drop attempts to set MIS_TOTAL parameter #11482

Merged
merged 5 commits into from
Aug 21, 2019
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 20 additions & 2 deletions Tools/autotest/common.py
Original file line number Diff line number Diff line change
Expand Up @@ -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):
peterbarker marked this conversation as resolved.
Show resolved Hide resolved
self.mavproxy.send("param set %s %s\n" % (name, str(value)))
returned_value = self.get_parameter(name)
delta = float(value) - returned_value
Expand Down Expand Up @@ -3070,6 +3070,20 @@ def run_tests(self, tests):

return True

def test_parameters(self):
peterbarker marked this conversation as resolved.
Show resolved Hide resolved
'''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 {}

Expand Down Expand Up @@ -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):
Expand Down
3 changes: 2 additions & 1 deletion libraries/AP_Mission/AP_Mission.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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),
peterbarker marked this conversation as resolved.
Show resolved Hide resolved

// @Param: RESTART
// @DisplayName: Mission Restart when entering Auto mode
Expand Down
5 changes: 4 additions & 1 deletion libraries/AP_Param/AP_Param.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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
Expand Down
10 changes: 8 additions & 2 deletions libraries/AP_Param/AP_Param.h
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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
///
Expand Down
4 changes: 4 additions & 0 deletions libraries/GCS_MAVLink/GCS.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
28 changes: 27 additions & 1 deletion libraries/GCS_MAVLink/GCS_Param.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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, &parameter_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);

Expand All @@ -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
*/
Expand Down