From 0a7d8a4933e307e0cd95700d7a50792d12d8548a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 16 Aug 2012 10:14:46 +1000
Subject: [PATCH] MAVLink: enable fetching of single parameters

this makes it possible for the GCS to fetch a single parameter by
name, which is useful for parameters that change in flight
---
 ArduCopter/GCS_Mavlink.pde | 33 +++++++++++++++++++++++++++++++++
 ArduPlane/GCS_Mavlink.pde  | 33 +++++++++++++++++++++++++++++++++
 2 files changed, 66 insertions(+)

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