diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index d54121cbc0..c644f008e4 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -15,6 +15,9 @@ static bool mavlink_active; // check if a message will fit in the payload space available #define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false +// prototype this for use inside the GCS class +static void gcs_send_text_fmt(const prog_char_t *fmt, ...); + /* !!NOTE!! @@ -1287,6 +1290,36 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: + { + // decode + mavlink_param_request_read_t packet; + mavlink_msg_param_request_read_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + if (packet.param_index != -1) { + gcs_send_text_P(SEVERITY_LOW, PSTR("Param by index not supported")); + break; + } + + enum ap_var_type p_type; + AP_Param *vp = AP_Param::find(packet.param_id, &p_type); + if (vp == NULL) { + gcs_send_text_fmt(PSTR("Unknown parameter %s"), packet.param_id); + break; + } + char param_name[ONBOARD_PARAM_NAME_LENGTH]; + vp->copy_name(param_name, sizeof(param_name), true); + + float value = vp->cast_to_float(p_type); + mavlink_msg_param_value_send( + chan, + param_name, + value, + mav_var_type(p_type), + -1, -1); + break; + } + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: // 45 { //send_text_P(SEVERITY_LOW,PSTR("waypoint clear all")); diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 9dade8d042..4eaa5c5073 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -13,6 +13,9 @@ static bool mavlink_active; // check if a message will fit in the payload space available #define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false +// prototype this for use inside the GCS class +void gcs_send_text_fmt(const prog_char_t *fmt, ...); + /* !!NOTE!! @@ -1255,6 +1258,36 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: + { + // decode + mavlink_param_request_read_t packet; + mavlink_msg_param_request_read_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + if (packet.param_index != -1) { + gcs_send_text_P(SEVERITY_LOW, PSTR("Param by index not supported")); + break; + } + + enum ap_var_type p_type; + AP_Param *vp = AP_Param::find(packet.param_id, &p_type); + if (vp == NULL) { + gcs_send_text_fmt(PSTR("Unknown parameter %s"), packet.param_id); + break; + } + char param_name[ONBOARD_PARAM_NAME_LENGTH]; + vp->copy_name(param_name, sizeof(param_name), true); + + float value = vp->cast_to_float(p_type); + mavlink_msg_param_value_send( + chan, + param_name, + value, + mav_var_type(p_type), + -1, -1); + break; + } + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { // decode