diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 573b653377..1f74971e68 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -786,7 +786,7 @@ void AP_Param::notify() const { param_header_type = info->type; } - send_parameter(name, (enum ap_var_type)param_header_type); + send_parameter(name, (enum ap_var_type)param_header_type, idx); } @@ -839,7 +839,7 @@ bool AP_Param::save(bool force_save) if (scan(&phdr, &ofs)) { // found an existing copy of the variable eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type)); - send_parameter(name, (enum ap_var_type)phdr.type); + send_parameter(name, (enum ap_var_type)phdr.type, idx); return true; } if (ofs == (uint16_t) ~0) { @@ -879,7 +879,7 @@ bool AP_Param::save(bool force_save) eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type)); eeprom_write_check(&phdr, ofs, sizeof(phdr)); - send_parameter(name, (enum ap_var_type)phdr.type); + send_parameter(name, (enum ap_var_type)phdr.type, idx); return true; } @@ -1650,22 +1650,37 @@ float AP_Param::get_default_value(const float *def_value_ptr) } -void AP_Param::send_parameter(char *name, enum ap_var_type param_header_type) const +void AP_Param::send_parameter(const char *name, enum ap_var_type var_type, uint8_t idx) const { - if (param_header_type != AP_PARAM_VECTOR3F) { + if (idx != 0 && var_type == AP_PARAM_VECTOR3F) { + var_type = AP_PARAM_FLOAT; + } + if (var_type > AP_PARAM_VECTOR3F) { + // invalid + return; + } + if (var_type != AP_PARAM_VECTOR3F) { // nice and simple for scalar types - GCS_MAVLINK::send_parameter_value_all(name, param_header_type, cast_to_float(param_header_type)); + GCS_MAVLINK::send_parameter_value_all(name, var_type, cast_to_float(var_type)); return; } - // for vectors we need to send 3 messages + // for vectors we need to send 3 messages. Note that we also come here for the case + // of a set of the first element of a AP_Vector3f. This happens as the ap->save() call can't + // distinguish between a vector and scalar save. It means that setting first element of a vector + // via MAVLink results in sending all 3 elements to the GCS const Vector3f &v = ((AP_Vector3f *)this)->get(); - char &name_axis = name[strlen(name)-1]; - GCS_MAVLINK::send_parameter_value_all(name, AP_PARAM_FLOAT, v.x); + char name2[AP_MAX_NAME_SIZE+1]; + strncpy(name2, name, AP_MAX_NAME_SIZE); + name2[AP_MAX_NAME_SIZE] = 0; + char &name_axis = name2[strlen(name)-1]; + + name_axis = 'X'; + GCS_MAVLINK::send_parameter_value_all(name2, AP_PARAM_FLOAT, v.x); name_axis = 'Y'; - GCS_MAVLINK::send_parameter_value_all(name, AP_PARAM_FLOAT, v.y); + GCS_MAVLINK::send_parameter_value_all(name2, AP_PARAM_FLOAT, v.y); name_axis = 'Z'; - GCS_MAVLINK::send_parameter_value_all(name, AP_PARAM_FLOAT, v.z); + GCS_MAVLINK::send_parameter_value_all(name2, AP_PARAM_FLOAT, v.z); } /* diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index a32d593acc..b89856ba62 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -210,10 +210,7 @@ public: /// Notify GCS of current parameter value /// void notify() const; - - // send a parameter to all GCS instances - void send_parameter(char *name, enum ap_var_type param_header_type) const; - + /// Save the current value of the variable to EEPROM. /// /// @param force_save If true then force save even if default @@ -440,6 +437,9 @@ private: static bool load_defaults_file(const char *filename); #endif + // send a parameter to all GCS instances + void send_parameter(const char *name, enum ap_var_type param_header_type, uint8_t idx) const; + static StorageAccess _storage; static uint16_t _num_vars; static uint16_t _parameter_count;