From 368363531fa6116bb067d4ab7b35be5cf4cb9a29 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 6 Oct 2015 20:54:05 +1100
Subject: [PATCH] GCS_MAVLink: provide facilities to send param values to all
 GCS

---
 libraries/GCS_MAVLink/GCS.h          |  7 ++++--
 libraries/GCS_MAVLink/GCS_Common.cpp | 35 +++++++++++++++++-----------
 2 files changed, 27 insertions(+), 15 deletions(-)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index 1ca2881f73..6bac32d597 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -166,6 +166,9 @@ public:
     */
     static void send_statustext_all(MAV_SEVERITY severity, const char *fmt, ...);
 
+    // send a PARAM_VALUE message to all active MAVLink connections.
+    static void send_parameter_value_all(const char *param_name, ap_var_type param_type, float param_value);
+    
     /*
       send a MAVLink message to all components with this vehicle's system id
       This is a no-op if no routes to components have been learned
@@ -209,10 +212,10 @@ private:
     ///
     /// @return         The number of reportable parameters.
     ///
-    uint16_t                    _count_parameters(); ///< count reportable
+    static uint16_t             _count_parameters(); ///< count reportable
                                                      // parameters
 
-    uint16_t                    _parameter_count;   ///< cache of reportable
+    static uint16_t             _parameter_count;   ///< cache of reportable
                                                     // parameters
 
     mavlink_channel_t           chan;
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index e892d7ffb1..60d94f9a70 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -26,6 +26,7 @@ extern const AP_HAL::HAL& hal;
 
 uint32_t GCS_MAVLINK::last_radio_status_remrssi_ms;
 uint8_t GCS_MAVLINK::mavlink_active = 0;
+uint16_t GCS_MAVLINK::_parameter_count;
 
 GCS_MAVLINK::GCS_MAVLINK() :
     waypoint_receive_timeout(5000)
@@ -591,19 +592,6 @@ void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *Data
     // save the change
     vp->save(force_save);
 
-    // Report back the new value if we accepted the change
-    // we send the value we actually set, which could be
-    // different from the value sent, in case someone sent
-    // a fractional value to an integer type
-    mavlink_msg_param_value_send_buf(
-        msg,
-        chan,
-        key,
-        vp->cast_to_float(var_type),
-        mav_var_type(var_type),
-        _count_parameters(),
-        -1);     // XXX we don't actually know what its index is...
-
     if (DataFlash != NULL) {
         DataFlash->Log_Write_Parameter(key, vp->cast_to_float(var_type));
     }
@@ -1181,6 +1169,27 @@ void GCS_MAVLINK::send_statustext_all(MAV_SEVERITY severity, const char *fmt, ..
     }
 }
 
+/*
+  send a parameter value message to all active MAVLink connections
+ */
+void GCS_MAVLINK::send_parameter_value_all(const char *param_name, ap_var_type param_type, float param_value)
+{
+    for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
+        if ((1U<<i) & mavlink_active) {
+            mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
+            if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_PARAM_VALUE_LEN) {
+                mavlink_msg_param_value_send(
+                    chan,
+                    param_name,
+                    param_value,
+                    mav_var_type(param_type),
+                    _count_parameters(),
+                    -1);
+            }
+        }
+    }
+}
+
 // report battery2 state
 void GCS_MAVLINK::send_battery2(const AP_BattMonitor &battery)
 {