From ff04871fc500692766a4f9ac428cb20762482a5d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 16 Mar 2016 17:03:30 +1100 Subject: [PATCH] AP_Param: fixed a reporting problem with AP_Vector3f if we load an element other than the X element of a Vector3f via MAVLink then the value reported back to the GCS would be at the wrong offset in memory. This led to some very confusing results for users --- libraries/AP_Param/AP_Param.cpp | 37 +++++++++++++++++++++++---------- libraries/AP_Param/AP_Param.h | 8 +++---- 2 files changed, 30 insertions(+), 15 deletions(-) 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;