diff --git a/libraries/DataFlash/DataFlash.cpp b/libraries/DataFlash/DataFlash.cpp index 36da43fd65..616c7a4f3b 100644 --- a/libraries/DataFlash/DataFlash.cpp +++ b/libraries/DataFlash/DataFlash.cpp @@ -232,9 +232,9 @@ void DataFlash_Class::internal_error() const { void DataFlash_Class::Log_Write(const char *name, const char *labels, const char *fmt, ...) { va_list arg_list; - - int16_t msg_type = msg_type_for_name(name, labels, fmt); - if (msg_type == -1) { + + struct log_write_fmt *f = msg_fmt_for_name(name, labels, fmt); + if (f == nullptr) { // unable to map name to a messagetype; could be out of // msgtypes, could be out of slots, ... internal_error(); @@ -242,46 +242,57 @@ void DataFlash_Class::Log_Write(const char *name, const char *labels, const char } for (uint8_t i=0; i<_next_backend; i++) { - va_start(arg_list, fmt); - if (backends[i]->Log_Write_Emit_FMT(msg_type)) { - backends[i]->Log_Write(msg_type, arg_list); - } else { - // backend failed to write format (and has never written it) + if (!(f->sent_mask & (1U<Log_Write_Emit_FMT(f->msg_type)) { + continue; + } + f->sent_mask |= (1U<Log_Write(f->msg_type, arg_list); va_end(arg_list); } } -int16_t DataFlash_Class::msg_type_for_name(const char *name, const char *labels, const char *fmt) +DataFlash_Class::log_write_fmt *DataFlash_Class::msg_fmt_for_name(const char *name, const char *labels, const char *fmt) { - for (uint8_t i=0; i<_log_write_fmt_count;i++) { - if (log_write_fmts[i].name == name) { // ptr comparison + struct log_write_fmt *f; + for (f = log_write_fmts; f; f=f->next) { + if (f->name == name) { // ptr comparison // already have an ID for this name: - return log_write_fmts[i].msg_type; + return f; } } - if (_log_write_fmt_count >= ARRAY_SIZE(log_write_fmts)) { - // out of slots to remember formats - return -1; + f = (struct log_write_fmt *)calloc(1, sizeof(*f)); + if (f == nullptr) { + // out of memory + return nullptr; } // no message type allocated for this name. Try to allocate one: int16_t msg_type = find_free_msg_type(); if (msg_type == -1) { - return -1; + free(f); + return nullptr; } - log_write_fmts[_log_write_fmt_count].msg_type = msg_type; - log_write_fmts[_log_write_fmt_count].name = name; - log_write_fmts[_log_write_fmt_count].fmt = fmt; - log_write_fmts[_log_write_fmt_count].labels = labels; + f->msg_type = msg_type; + f->name = name; + f->fmt = fmt; + f->labels = labels; + int16_t tmp = Log_Write_calc_msg_len(fmt); if (tmp == -1) { - return -1; + free(f); + return nullptr; } - log_write_fmts[_log_write_fmt_count].msg_len = tmp; - _log_write_fmt_count++; - return msg_type; + f->msg_len = tmp; + + // add to front of list + f->next = log_write_fmts; + log_write_fmts = f; + + return f; } // returns true if the msg_type is already taken @@ -295,9 +306,10 @@ bool DataFlash_Class::msg_type_in_use(const uint8_t msg_type) const return true; } } - // check the write messages we've already put out: - for (uint8_t i=0; i<_log_write_fmt_count; i++) { - if (log_write_fmts[i].msg_type == msg_type) { + + struct log_write_fmt *f; + for (f = log_write_fmts; f; f=f->next) { + if (f->msg_type == msg_type) { return true; } } @@ -319,22 +331,17 @@ int16_t DataFlash_Class::find_free_msg_type() const bool DataFlash_Class::fill_log_write_logstructure(struct LogStructure &logstruct, const uint8_t msg_type) const { // find log structure information corresponding to msg_type: - uint8_t i; - for (i=0; i<_log_write_fmt_count; i++) { - if(log_write_fmts[i].msg_type == msg_type) { + struct log_write_fmt *f; + for (f = log_write_fmts; f; f=f->next) { + if(f->msg_type == msg_type) { break; } } - if (i >= _log_write_fmt_count) { - // asked to fill for a type we don't recognise - internal_error(); - return false; - } logstruct.msg_type = msg_type; - strncpy((char*)logstruct.name, log_write_fmts[i].name, sizeof(logstruct.name)); /* cast away the "const" (*gulp*) */ - strncpy((char*)logstruct.format, log_write_fmts[i].fmt, sizeof(logstruct.format)); - strncpy((char*)logstruct.labels, log_write_fmts[i].labels, sizeof(logstruct.labels)); - logstruct.msg_len = log_write_fmts[i].msg_len; + strncpy((char*)logstruct.name, f->name, sizeof(logstruct.name)); /* cast away the "const" (*gulp*) */ + strncpy((char*)logstruct.format, f->fmt, sizeof(logstruct.format)); + strncpy((char*)logstruct.labels, f->labels, sizeof(logstruct.labels)); + logstruct.msg_len = f->msg_len; return true; } diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index f1a8b41b4c..1ceb0fb6cf 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -5,9 +5,6 @@ /* ************************************************************ */ #pragma once -// maximum number of dynamic Log_Write formats supported -#define MAX_LOG_WRITE_FMT_COUNT 5 - #include #include #include @@ -216,17 +213,19 @@ private: // this structure looks much like struct LogStructure in // LogStructure.h, however we need to remember a pointer value for // efficiency of finding message types - struct { + struct log_write_fmt { + struct log_write_fmt *next; uint8_t msg_type; uint8_t msg_len; + uint8_t sent_mask; // bitmask of backends sent to const char *name; const char *fmt; const char *labels; - } log_write_fmts[MAX_LOG_WRITE_FMT_COUNT]; - uint8_t _log_write_fmt_count = 0; // number of formats currently known + } *log_write_fmts; - // return (possibly allocating) a msg_type value corresponding to name - int16_t msg_type_for_name(const char *name, const char *labels, const char *fmt); + // return (possibly allocating) a log_write_fmt for a name + struct log_write_fmt *msg_fmt_for_name(const char *name, const char *labels, const char *fmt); + // returns true if msg_type is associated with a message bool msg_type_in_use(uint8_t msg_type) const; diff --git a/libraries/DataFlash/DataFlash_Backend.cpp b/libraries/DataFlash/DataFlash_Backend.cpp index 4d574b5bb6..43dbd60907 100644 --- a/libraries/DataFlash/DataFlash_Backend.cpp +++ b/libraries/DataFlash/DataFlash_Backend.cpp @@ -53,7 +53,6 @@ void DataFlash_Backend::periodic_tasks() void DataFlash_Backend::start_new_log_reset_variables() { _startup_messagewriter->reset(); - memset(log_write_fmts_sent,0,sizeof(log_write_fmts_sent)); } void DataFlash_Backend::internal_error() { @@ -118,21 +117,6 @@ void DataFlash_Backend::WriteMoreStartupMessages() bool DataFlash_Backend::Log_Write_Emit_FMT(uint8_t msg_type) { - for(uint8_t i=0;i<_written_log_write_fmt_count;i++) { - if(log_write_fmts_sent[i] == msg_type) { - // have already emitted this format - return true; - } - } - - // have not emitted this format; try to do so now. - if (_written_log_write_fmt_count >= ARRAY_SIZE(log_write_fmts_sent)) { - // this is a bug; frontend should never ask us to write more - // formats than we have room to store "sent" markers for - internal_error(); - return false; - } - // get log structure from front end: struct LogStructure logstruct = { // these will be overwritten, but need to keep the compiler happy: @@ -154,8 +138,6 @@ bool DataFlash_Backend::Log_Write_Emit_FMT(uint8_t msg_type) return false; } - log_write_fmts_sent[_written_log_write_fmt_count] = msg_type; - _written_log_write_fmt_count++; return true; } @@ -166,10 +148,11 @@ bool DataFlash_Backend::Log_Write(const uint8_t msg_type, va_list arg_list, bool // abstraction we could do WriteBytes() here instead? const char *fmt = NULL; uint8_t msg_len; - for (uint8_t i=0; i<_written_log_write_fmt_count;i++) { - if (_front.log_write_fmts[i].msg_type == msg_type) { - fmt = _front.log_write_fmts[i].fmt; - msg_len = _front.log_write_fmts[i].msg_len; + DataFlash_Class::log_write_fmt *f; + for (f = _front.log_write_fmts; f; f=f->next) { + if (f->msg_type == msg_type) { + fmt = f->fmt; + msg_len = f->msg_len; break; } } diff --git a/libraries/DataFlash/DataFlash_Backend.h b/libraries/DataFlash/DataFlash_Backend.h index 42ac0c1fcd..f15fd93c10 100644 --- a/libraries/DataFlash/DataFlash_Backend.h +++ b/libraries/DataFlash/DataFlash_Backend.h @@ -158,10 +158,4 @@ private: uint32_t _last_periodic_1Hz; uint32_t _last_periodic_10Hz; - - // Log_Write support - uint8_t _written_log_write_fmt_count; - - // array containing msg_types of formats already emitted - uint8_t log_write_fmts_sent[MAX_LOG_WRITE_FMT_COUNT]; }; diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 27d5c916f6..8f22db7f89 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -647,6 +647,10 @@ void DataFlash_Class::StartNewLog(void) for (uint8_t i=0; i<_next_backend; i++) { backends[i]->start_new_log(); } + // reset sent masks + for (struct log_write_fmt *f = log_write_fmts; f; f=f->next) { + f->sent_mask = 0; + } } /*