AP_Param: provide facility to notify GCS of param set

libraries/AP_Param/AP_Param.cpp

@@ -371,7 +371,7 @@ const struct AP_Param::Info *AP_Param::find_var_info_group(const struct GroupInf
 // find the info structure for a variable
 const struct AP_Param::Info *AP_Param::find_var_info(uint32_t *                 group_element,
                                                      const struct GroupInfo **  group_ret,
-                                                     uint8_t *                  idx)
+                                                     uint8_t *                  idx) const
 {
     for (uint8_t i=0; i<_num_vars; i++) {
         uint8_t type = PGM_UINT8(&_var_info[i].type);
@@ -530,6 +530,11 @@ void AP_Param::copy_name_token(const ParamToken &token, char *buffer, size_t buf
         Debug("no info found");
         return;
     }
+    copy_name_info(info, ginfo, idx, buffer, buffer_size, force_scalar);
+}
+
+void AP_Param::copy_name_info(const struct AP_Param::Info *info, const struct GroupInfo *ginfo, uint8_t idx, char *buffer, size_t buffer_size, bool force_scalar) const
+{
     strncpy(buffer, info->name, buffer_size);
     if (ginfo != NULL) {
         uint8_t len = strnlen(buffer, buffer_size);
@@ -672,6 +677,32 @@ AP_Param::find_object(const char *name)
     return NULL;
 }
 
+// notify GCS of current value of parameter
+void AP_Param::notify() const {
+    uint32_t group_element = 0;
+    const struct GroupInfo *ginfo;
+    uint8_t idx;
+
+    const struct AP_Param::Info *info = find_var_info(&group_element, &ginfo, &idx);
+    if (info == NULL) {
+        // this is probably very bad
+        return;
+    }
+
+    char name[AP_MAX_NAME_SIZE+1];
+    copy_name_info(info, ginfo, idx, name, sizeof(name), true);
+
+    uint32_t param_header_type;
+    if (ginfo != NULL) {
+        param_header_type = PGM_UINT8(&ginfo->type);
+    } else {
+        param_header_type = PGM_UINT8(&info->type);
+    }
+
+    GCS_MAVLINK::send_parameter_value_all(name, (enum ap_var_type)info->type,
+                                          cast_to_float((enum ap_var_type)param_header_type));
+}
+
 
 // Save the variable to EEPROM, if supported
 //

libraries/AP_Param/AP_Param.h

@@ -141,6 +141,11 @@ public:
     /// @param	buffer			The destination buffer
     /// @param	bufferSize		Total size of the destination buffer.
     ///
+    void copy_name_info(const struct AP_Param::Info *info, const struct GroupInfo *ginfo, uint8_t idx, char *buffer, size_t bufferSize, bool force_scalar=false) const;
+    /// Copy the variable's name, prefixed by any containing group name, to a
+    /// buffer.
+    ///
+    /// Uses token to look up AP_Param::Info for the variable
     void copy_name_token(const ParamToken &token, char *buffer, size_t bufferSize, bool force_scalar=false) const;
 
     /// Find a variable by name.
@@ -170,6 +175,10 @@ public:
     ///
     static AP_Param * find_object(const char *name);
 
+    /// Notify GCS of current parameter value
+    ///
+    void notify() const;
+
     /// Save the current value of the variable to EEPROM.
     ///
     /// @param  force_save     If true then force save even if default
@@ -319,7 +328,7 @@ private:
     const struct Info *         find_var_info(
                                     uint32_t *                group_element,
                                     const struct GroupInfo ** group_ret,
-                                    uint8_t *                 idx);
+                                    uint8_t *                 idx) const;
     const struct Info *			find_var_info_token(const ParamToken &token,
                                                     uint32_t *                 group_element,
                                                     const struct GroupInfo **  group_ret,
@@ -432,6 +441,13 @@ public:
             set(v);
         }
     }
+    
+    /// Value setter - set value, tell GCS
+    ///
+    void set_and_notify(const T &v) {
+        set(v);
+        notify();
+    }
 
     /// Combined set and save
     ///