Browse Source

ACM: allow fetch of parameters by index

this makes it more efficient to re-fetch parameters that are missing
mission-4.1.18
Andrew Tridgell 12 years ago
parent
commit
dbdb3e1194
  1. 17
      ArduCopter/GCS_Mavlink.pde

17
ArduCopter/GCS_Mavlink.pde

@ -1358,17 +1358,21 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_param_request_read_t packet; mavlink_param_request_read_t packet;
mavlink_msg_param_request_read_decode(msg, &packet); mavlink_msg_param_request_read_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break; if (mavlink_check_target(packet.target_system,packet.target_component)) break;
enum ap_var_type p_type;
AP_Param *vp;
if (packet.param_index != -1) { if (packet.param_index != -1) {
gcs_send_text_P(SEVERITY_LOW, PSTR("Param by index not supported")); vp = AP_Param::find_by_index(packet.param_index, &p_type);
if (vp == NULL) {
gcs_send_text_fmt(PSTR("Unknown parameter index %d"), packet.param_index);
break; break;
} }
} else {
enum ap_var_type p_type; vp = AP_Param::find(packet.param_id, &p_type);
AP_Param *vp = AP_Param::find(packet.param_id, &p_type);
if (vp == NULL) { if (vp == NULL) {
gcs_send_text_fmt(PSTR("Unknown parameter %s"), packet.param_id); gcs_send_text_fmt(PSTR("Unknown parameter %.16s"), packet.param_id);
break; break;
} }
}
char param_name[AP_MAX_NAME_SIZE]; char param_name[AP_MAX_NAME_SIZE];
vp->copy_name(param_name, sizeof(param_name), true); vp->copy_name(param_name, sizeof(param_name), true);
@ -1378,7 +1382,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
param_name, param_name,
value, value,
mav_var_type(p_type), mav_var_type(p_type),
-1, -1); _count_parameters(),
packet.param_index);
break; break;
} }

Loading…
Cancel
Save