diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index b366c4a96a..300ad7cb6e 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/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 // diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index 82de0f2465..4287ef3607 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/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 ///