From c6c56033829c4b948da5c1f462db631716b64830 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 24 Jan 2018 15:42:09 +1100 Subject: [PATCH] AP_Param: added back key dumping code this is essential for working out conversion tables. --- libraries/AP_Param/AP_Param.cpp | 57 +++++++++++++++++++++++++++++++++ libraries/AP_Param/AP_Param.h | 20 ++++++++++++ 2 files changed, 77 insertions(+) diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 29be9dc4a8..e84f5bbeca 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -2193,3 +2193,60 @@ bool AP_Param::set_and_save_by_name(const char *name, float value) // not a supported type return false; } + +#if AP_PARAM_KEY_DUMP +/* + do not remove this show_all() code, it is essential for debugging + and creating conversion tables + */ + +// print the value of all variables +void AP_Param::show(const AP_Param *ap, const char *s, + enum ap_var_type type, AP_HAL::BetterStream *port) +{ + switch (type) { + case AP_PARAM_INT8: + port->printf("%s: %d\n", s, (int)((AP_Int8 *)ap)->get()); + break; + case AP_PARAM_INT16: + port->printf("%s: %d\n", s, (int)((AP_Int16 *)ap)->get()); + break; + case AP_PARAM_INT32: + port->printf("%s: %ld\n", s, (long)((AP_Int32 *)ap)->get()); + break; + case AP_PARAM_FLOAT: + port->printf("%s: %f\n", s, (double)((AP_Float *)ap)->get()); + break; + default: + break; + } +} + +// print the value of all variables +void AP_Param::show(const AP_Param *ap, const ParamToken &token, + enum ap_var_type type, AP_HAL::BetterStream *port) +{ + char s[AP_MAX_NAME_SIZE+1]; + ap->copy_name_token(token, s, sizeof(s), true); + s[AP_MAX_NAME_SIZE] = 0; + show(ap, s, type, port); +} + +// print the value of all variables +void AP_Param::show_all(AP_HAL::BetterStream *port, bool showKeyValues) +{ + ParamToken token; + AP_Param *ap; + enum ap_var_type type; + + for (ap=AP_Param::first(&token, &type); + ap; + ap=AP_Param::next_scalar(&token, &type)) { + if (showKeyValues) { + port->printf("Key %i: Index %i: GroupElement %i : ", token.key, token.idx, token.group_element); + } + show(ap, token, type, port); + } +} +#endif // AP_PARAM_KEY_DUMP + diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index 2ec6f11bc1..623fe4657f 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -31,6 +31,9 @@ #define AP_MAX_NAME_SIZE 16 +// optionally enable debug code for dumping keys +#define AP_PARAM_KEY_DUMP 0 + /* maximum size of embedded parameter file */ @@ -413,6 +416,23 @@ public: // check if a given frame type should be included static bool check_frame_type(uint16_t flags); + +#if AP_PARAM_KEY_DUMP + /// print the value of all variables + static void show_all(AP_HAL::BetterStream *port, bool showKeyValues=false); + + /// print the value of one variable + static void show(const AP_Param *param, + const char *name, + enum ap_var_type ptype, + AP_HAL::BetterStream *port); + + /// print the value of one variable + static void show(const AP_Param *param, + const ParamToken &token, + enum ap_var_type ptype, + AP_HAL::BetterStream *port); +#endif // AP_PARAM_KEY_DUMP private: /// EEPROM header