From 63e03622d4392365e9bada235ba5e115e35d7d61 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Mon, 13 Jun 2022 20:02:15 +0100 Subject: [PATCH] AP_Logger: log default param values --- libraries/AP_Logger/AP_Logger.cpp | 2 +- libraries/AP_Logger/AP_Logger_Backend.h | 5 +++-- libraries/AP_Logger/LogFile.cpp | 10 ++++++---- libraries/AP_Logger/LogStructure.h | 4 +++- libraries/AP_Logger/LoggerMessageWriter.cpp | 8 +++++--- libraries/AP_Logger/LoggerMessageWriter.h | 1 + 6 files changed, 19 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp index 50e3b37c59..5829f2e6e1 100644 --- a/libraries/AP_Logger/AP_Logger.cpp +++ b/libraries/AP_Logger/AP_Logger.cpp @@ -865,7 +865,7 @@ void AP_Logger::Write_Mode(uint8_t mode, const ModeReason reason) void AP_Logger::Write_Parameter(const char *name, float value) { - FOR_EACH_BACKEND(Write_Parameter(name, value)); + FOR_EACH_BACKEND(Write_Parameter(name, value, quiet_nanf())); } void AP_Logger::Write_Mission_Cmd(const AP_Mission &mission, diff --git a/libraries/AP_Logger/AP_Logger_Backend.h b/libraries/AP_Logger/AP_Logger_Backend.h index 1590435ad8..0b0d56e825 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.h +++ b/libraries/AP_Logger/AP_Logger_Backend.h @@ -128,10 +128,11 @@ public: bool Write_Mission_Cmd(const AP_Mission &mission, const AP_Mission::Mission_Command &cmd); bool Write_Mode(uint8_t mode, const ModeReason reason); - bool Write_Parameter(const char *name, float value); + bool Write_Parameter(const char *name, float value, float default_val); bool Write_Parameter(const AP_Param *ap, const AP_Param::ParamToken &token, - enum ap_var_type type); + enum ap_var_type type, + float default_val); bool Write_VER(); uint32_t num_dropped(void) const { diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index eb135f6f2c..41a370588d 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -103,13 +103,14 @@ bool AP_Logger_Backend::Write_Format_Units(const struct LogStructure *s) /* write a parameter to the log */ -bool AP_Logger_Backend::Write_Parameter(const char *name, float value) +bool AP_Logger_Backend::Write_Parameter(const char *name, float value, float default_val) { struct log_Parameter pkt{ LOG_PACKET_HEADER_INIT(LOG_PARAMETER_MSG), time_us : AP_HAL::micros64(), name : {}, - value : value + value : value, + default_value : default_val }; strncpy_noterm(pkt.name, name, sizeof(pkt.name)); return WriteCriticalBlock(&pkt, sizeof(pkt)); @@ -120,11 +121,12 @@ bool AP_Logger_Backend::Write_Parameter(const char *name, float value) */ bool AP_Logger_Backend::Write_Parameter(const AP_Param *ap, const AP_Param::ParamToken &token, - enum ap_var_type type) + enum ap_var_type type, + float default_val) { char name[16]; ap->copy_name_token(token, &name[0], sizeof(name), true); - return Write_Parameter(name, ap->cast_to_float(type)); + return Write_Parameter(name, ap->cast_to_float(type), default_val); } // Write an RCIN packet diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 48d1aca4aa..bf3863bde5 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -205,6 +205,7 @@ struct PACKED log_Parameter { uint64_t time_us; char name[16]; float value; + float default_value; }; struct PACKED log_DSF { @@ -920,6 +921,7 @@ struct PACKED log_VER { // @Field: TimeUS: Time since system startup // @Field: Name: parameter name // @Field: Value: parameter value +// @Field: Default: default parameter value for this board and config // @LoggerMessage: PIDR,PIDP,PIDY,PIDA,PIDS,PIDN,PIDE // @Description: Proportional/Integral/Derivative gain values for Roll/Pitch/Yaw/Altitude/Steering @@ -1213,7 +1215,7 @@ struct PACKED log_VER { { LOG_MULT_MSG, sizeof(log_Format_Multiplier), \ "MULT", "Qbd", "TimeUS,Id,Mult", "s--","F--" }, \ { LOG_PARAMETER_MSG, sizeof(log_Parameter), \ - "PARM", "QNf", "TimeUS,Name,Value", "s--", "F--" }, \ + "PARM", "QNff", "TimeUS,Name,Value,Default", "s---", "F---" }, \ LOG_STRUCTURE_FROM_GPS \ { LOG_MESSAGE_MSG, sizeof(log_Message), \ "MSG", "QZ", "TimeUS,Message", "s-", "F-"}, \ diff --git a/libraries/AP_Logger/LoggerMessageWriter.cpp b/libraries/AP_Logger/LoggerMessageWriter.cpp index 6fdb504f92..429062489b 100644 --- a/libraries/AP_Logger/LoggerMessageWriter.cpp +++ b/libraries/AP_Logger/LoggerMessageWriter.cpp @@ -50,7 +50,8 @@ void LoggerMessageWriter_DFLogStart::reset() _next_unit_to_send = 0; _next_multiplier_to_send = 0; _next_format_unit_to_send = 0; - ap = AP_Param::first(&token, &type); + param_default = AP::logger().quiet_nanf(); + ap = AP_Param::first(&token, &type, ¶m_default); } bool LoggerMessageWriter_DFLogStart::out_of_time_for_writing_messages() const @@ -87,10 +88,11 @@ void LoggerMessageWriter_DFLogStart::process() case Stage::PARMS: while (ap) { - if (!_logger_backend->Write_Parameter(ap, token, type)) { + if (!_logger_backend->Write_Parameter(ap, token, type, param_default)) { return; } - ap = AP_Param::next_scalar(&token, &type); + param_default = AP::logger().quiet_nanf(); + ap = AP_Param::next_scalar(&token, &type, ¶m_default); } _params_done = true; diff --git a/libraries/AP_Logger/LoggerMessageWriter.h b/libraries/AP_Logger/LoggerMessageWriter.h index 3869a0228a..f052f0cce4 100644 --- a/libraries/AP_Logger/LoggerMessageWriter.h +++ b/libraries/AP_Logger/LoggerMessageWriter.h @@ -140,6 +140,7 @@ private: AP_Param::ParamToken token; AP_Param *ap; + float param_default; enum ap_var_type type;