GCS_MAVLink: deny setting MIS_TOTAL parameter

Closes #11413

GCS_MAVLink: conform to parameter protocol requirements while ignoring sets

GCS_MAVLink: use internal-use-only param bit
This commit is contained in:
Peter Barker 2019-06-04 14:15:36 +10:00 committed by Peter Barker
parent cce85cb418
commit 4f9d1ada33
2 changed files with 31 additions and 1 deletions

View File

@ -116,6 +116,10 @@ public:
virtual uint8_t sysid_my_gcs() const = 0; virtual uint8_t sysid_my_gcs() const = 0;
virtual bool sysid_enforce() const { return false; } 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 // NOTE! The streams enum below and the
// set of AP_Int16 stream rates _must_ be // set of AP_Int16 stream rates _must_ be
// kept in the same order // kept in the same order

View File

@ -268,12 +268,24 @@ void GCS_MAVLINK::handle_param_set(const mavlink_message_t &msg)
key[AP_MAX_NAME_SIZE] = 0; key[AP_MAX_NAME_SIZE] = 0;
// find existing param so we can get the old value // 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) { if (vp == nullptr) {
return; return;
} }
float old_value = vp->cast_to_float(var_type); 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 // set the value
vp->set_float(packet.param_value, var_type); 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 send a parameter value message to all active MAVLink connections
*/ */