From 1a45aa12f556e00e4022c0bf85ae391943f5f0ad Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Tue, 26 Jun 2018 08:54:36 -0700 Subject: [PATCH] AP_Param: warn on last pass only about unknown parameters Do not warn about unknown parameters on the first pass, i.e. when AP_Param:load_all() is called. This is because we may still not know about dynamically loaded parameters. When we call the second (last) time, we expect to already know all possible parameters, so print a warning to both console and the debug terminal. --- ArduCopter/system.cpp | 2 +- ArduPlane/system.cpp | 2 +- libraries/AP_Param/AP_Param.cpp | 43 +++++++++++++++++++++------------ libraries/AP_Param/AP_Param.h | 8 +++--- 4 files changed, 34 insertions(+), 21 deletions(-) diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index e3e766e2eb..f0d3ee9d6c 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -611,7 +611,7 @@ void Copter::allocate_motors(void) #endif // reload lines from the defaults file that may now be accessible - AP_Param::reload_defaults_file(); + AP_Param::reload_defaults_file(true); // now setup some frame-class specific defaults switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) { diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 288403e205..5d544d9155 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -184,7 +184,7 @@ void Plane::init_ardupilot() quadplane.setup(); - AP_Param::reload_defaults_file(); + AP_Param::reload_defaults_file(true); startup_ground(); diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 824ba56c47..9eb9358c0c 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -1274,7 +1274,7 @@ bool AP_Param::load_all() struct Param_header phdr; uint16_t ofs = sizeof(AP_Param::EEPROM_header); - reload_defaults_file(); + reload_defaults_file(false); while (ofs < _storage.size()) { _storage.read_block(&phdr, ofs, sizeof(phdr)); @@ -1302,12 +1302,14 @@ bool AP_Param::load_all() } /* - reload from hal.util defaults file + * reload from hal.util defaults file or embedded param region + * @last_pass: if this is the last pass on defaults - unknown parameters are + * ignored but if this is set a warning will be emitted */ -void AP_Param::reload_defaults_file() +void AP_Param::reload_defaults_file(bool last_pass) { if (param_defaults_data.length != 0) { - load_embedded_param_defaults(); + load_embedded_param_defaults(last_pass); return; } @@ -1318,7 +1320,7 @@ void AP_Param::reload_defaults_file() */ const char *default_file = hal.util->get_custom_defaults_file(); if (default_file) { - if (load_defaults_file(default_file)) { + if (load_defaults_file(default_file, last_pass)) { printf("Loaded defaults from %s\n", default_file); } else { printf("Failed to load defaults from %s\n", default_file); @@ -1815,7 +1817,7 @@ bool AP_Param::count_defaults_in_file(const char *filename, uint16_t &num_defaul return true; } -bool AP_Param::read_param_defaults_file(const char *filename) +bool AP_Param::read_param_defaults_file(const char *filename, bool last_pass) { FILE *f = fopen(filename, "r"); if (f == nullptr) { @@ -1834,8 +1836,13 @@ bool AP_Param::read_param_defaults_file(const char *filename) enum ap_var_type var_type; AP_Param *vp = find(pname, &var_type); if (!vp) { - ::printf("Ignored unknown param %s in defaults file %s\n", - pname, filename); + if (last_pass) { + ::printf("Ignored unknown param %s in defaults file %s\n", + pname, filename); + hal.console->printf( + "Ignored unknown param %s in defaults file %s\n", + pname, filename); + } continue; } param_overrides[idx].object_ptr = vp; @@ -1852,7 +1859,7 @@ bool AP_Param::read_param_defaults_file(const char *filename) /* load a default set of parameters from a file */ -bool AP_Param::load_defaults_file(const char *filename) +bool AP_Param::load_defaults_file(const char *filename, bool last_pass) { if (filename == nullptr) { return false; @@ -1894,7 +1901,7 @@ bool AP_Param::load_defaults_file(const char *filename) for (char *pname = strtok_r(mutable_filename, ",", &saveptr); pname != nullptr; pname = strtok_r(nullptr, ",", &saveptr)) { - if (!read_param_defaults_file(pname)) { + if (!read_param_defaults_file(pname, last_pass)) { free(mutable_filename); return false; } @@ -1957,11 +1964,12 @@ bool AP_Param::count_embedded_param_defaults(uint16_t &count) return true; } - /* - load a default set of parameters from a embedded parameter region + * load a default set of parameters from a embedded parameter region + * @last_pass: if this is the last pass on defaults - unknown parameters are + * ignored but if this is set a warning will be emitted */ -void AP_Param::load_embedded_param_defaults() +void AP_Param::load_embedded_param_defaults(bool last_pass) { if (param_overrides != nullptr) { free(param_overrides); @@ -2015,8 +2023,13 @@ void AP_Param::load_embedded_param_defaults() enum ap_var_type var_type; AP_Param *vp = find(pname, &var_type); if (!vp) { - ::printf("Ignored unknown param %s from embedded region (offset=%u)\n", - pname, ptr - param_defaults_data.data); + if (last_pass) { + ::printf("Ignored unknown param %s from embedded region (offset=%u)\n", + pname, ptr - param_defaults_data.data); + hal.console->printf( + "Ignored unknown param %s from embedded region (offset=%u)\n", + pname, ptr - param_defaults_data.data); + } continue; } param_overrides[idx].object_ptr = vp; diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index 272cfc34cb..f4c753b971 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -327,7 +327,7 @@ public: /// reoad the hal.util defaults file. Called after pointer parameters have been allocated /// - static void reload_defaults_file(); + static void reload_defaults_file(bool last_pass); static void load_object_from_eeprom(const void *object_pointer, const struct GroupInfo *group_info); @@ -574,15 +574,15 @@ private: load a parameter defaults file. This happens as part of load_all() */ static bool count_defaults_in_file(const char *filename, uint16_t &num_defaults); - static bool read_param_defaults_file(const char *filename); - static bool load_defaults_file(const char *filename); + static bool read_param_defaults_file(const char *filename, bool last_pass); + static bool load_defaults_file(const char *filename, bool last_pass); #endif /* load defaults from embedded parameters */ static bool count_embedded_param_defaults(uint16_t &count); - static void load_embedded_param_defaults(); + static void load_embedded_param_defaults(bool last_pass); // send a parameter to all GCS instances void send_parameter(const char *name, enum ap_var_type param_header_type, uint8_t idx) const;