diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index ca86b2c2e1..b8c3ee16ef 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -1254,6 +1254,7 @@ void Logger::start_log_file(LogType type) if (type == LogType::Full) { write_parameters(type); + write_parameter_defaults(type); write_perf_data(true); write_console_output(); } @@ -1305,6 +1306,7 @@ void Logger::start_log_mavlink() write_version(LogType::Full); write_formats(LogType::Full); write_parameters(LogType::Full); + write_parameter_defaults(LogType::Full); write_perf_data(true); write_console_output(); write_all_add_logged_msg(LogType::Full); @@ -1738,6 +1740,8 @@ void Logger::write_header(LogType type) // write the Flags message: this MUST be written right after the ulog header ulog_message_flag_bits_s flag_bits{}; + flag_bits.compat_flags[0] = ULOG_COMPAT_FLAG0_DEFAULT_PARAMETERS_MASK; + flag_bits.msg_size = sizeof(flag_bits) - ULOG_MSG_HEADER_LEN; flag_bits.msg_type = static_cast(ULogMessageType::FLAG_BITS); @@ -1822,6 +1826,100 @@ void Logger::write_version(LogType type) } } +void Logger::write_parameter_defaults(LogType type) +{ + _writer.lock(); + ulog_message_parameter_default_header_s msg = {}; + uint8_t *buffer = reinterpret_cast(&msg); + + msg.msg_type = static_cast(ULogMessageType::PARAMETER_DEFAULT); + int param_idx = 0; + param_t param = 0; + + do { + // skip over all parameters which are not used + do { + param = param_for_index(param_idx); + ++param_idx; + } while (param != PARAM_INVALID && !param_used(param)); + + // save parameters which are valid AND used AND not volatile + if (param != PARAM_INVALID) { + + if (param_is_volatile(param)) { + continue; + } + + // get parameter type and size + const char *type_str; + param_type_t ptype = param_type(param); + size_t value_size = 0; + + uint8_t default_value[math::max(sizeof(float), sizeof(int32_t))]; + uint8_t system_default_value[sizeof(default_value)]; + uint8_t value[sizeof(default_value)]; + + switch (ptype) { + case PARAM_TYPE_INT32: + type_str = "int32_t"; + value_size = sizeof(int32_t); + param_get(param, (int32_t *)&value); + break; + + case PARAM_TYPE_FLOAT: + type_str = "float"; + value_size = sizeof(float); + param_get(param, (float *)&value); + break; + + default: + continue; + } + + // format parameter key (type and name) + msg.key_len = snprintf(msg.key, sizeof(msg.key), "%s %s", type_str, param_name(param)); + size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len; + + if (param_get_default_value(param, &default_value) != 0) { + continue; + } + + if (param_get_system_default_value(param, &system_default_value) != 0) { + continue; + } + + msg_size += value_size; + msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; + + // write the system/airframe default if different from the current value + if (memcmp(&system_default_value, &default_value, value_size) == 0) { + // if the system and airframe defaults are equal, we can combine them + if (memcmp(&value, &default_value, value_size) != 0) { + memcpy(&buffer[msg_size - value_size], default_value, value_size); + msg.default_types = ulog_parameter_default_type_t::current_setup | ulog_parameter_default_type_t::system; + write_message(type, buffer, msg_size); + } + + } else { + if (memcmp(&value, &default_value, value_size) != 0) { + memcpy(&buffer[msg_size - value_size], default_value, value_size); + msg.default_types = ulog_parameter_default_type_t::current_setup; + write_message(type, buffer, msg_size); + } + + if (memcmp(&value, &system_default_value, value_size) != 0) { + memcpy(&buffer[msg_size - value_size], system_default_value, value_size); + msg.default_types = ulog_parameter_default_type_t::system; + write_message(type, buffer, msg_size); + } + } + } + } while ((param != PARAM_INVALID) && (param_idx < (int) param_count())); + + _writer.unlock(); + _writer.notify(); +} + void Logger::write_parameters(LogType type) { _writer.lock(); @@ -1833,7 +1931,7 @@ void Logger::write_parameters(LogType type) param_t param = 0; do { - // skip over all parameters which are not invalid and not used + // skip over all parameters which are not used do { param = param_for_index(param_idx); ++param_idx; @@ -1902,7 +2000,7 @@ void Logger::write_changed_parameters(LogType type) param_t param = 0; do { - // skip over all parameters which are not invalid and not used + // skip over all parameters which are not used do { param = param_for_index(param_idx); ++param_idx; diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 01129dfbba..f73497b513 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -258,6 +258,7 @@ private: void write_info_template(LogType type, const char *name, T value, const char *type_str); void write_parameters(LogType type); + void write_parameter_defaults(LogType type); void write_changed_parameters(LogType type); diff --git a/src/modules/logger/messages.h b/src/modules/logger/messages.h index 4b877893b3..c6512db00b 100644 --- a/src/modules/logger/messages.h +++ b/src/modules/logger/messages.h @@ -33,12 +33,15 @@ #pragma once +#include + enum class ULogMessageType : uint8_t { FORMAT = 'F', DATA = 'D', INFO = 'I', INFO_MULTIPLE = 'M', PARAMETER = 'P', + PARAMETER_DEFAULT = 'Q', ADD_LOGGED_MSG = 'A', REMOVE_LOGGED_MSG = 'R', SYNC = 'S', @@ -152,9 +155,30 @@ struct ulog_message_parameter_header_s { char key[255]; }; +enum class ulog_parameter_default_type_t : uint8_t { + system = (1 << 0), + current_setup = (1 << 1) //airframe default +}; + +inline ulog_parameter_default_type_t operator|(ulog_parameter_default_type_t a, ulog_parameter_default_type_t b) +{ + return static_cast(static_cast(a) | static_cast(b)); +} + +struct ulog_message_parameter_default_header_s { + uint16_t msg_size; + uint8_t msg_type = static_cast(ULogMessageType::PARAMETER_DEFAULT); + + ulog_parameter_default_type_t default_types; + uint8_t key_len; + char key[255]; +}; + #define ULOG_INCOMPAT_FLAG0_DATA_APPENDED_MASK (1<<0) +#define ULOG_COMPAT_FLAG0_DEFAULT_PARAMETERS_MASK (1<<0) + struct ulog_message_flag_bits_s { uint16_t msg_size; uint8_t msg_type = static_cast(ULogMessageType::FLAG_BITS);