diff --git a/libraries/DataFlash/DataFlash.cpp b/libraries/DataFlash/DataFlash.cpp index dc9636ad98..d4084cca7e 100644 --- a/libraries/DataFlash/DataFlash.cpp +++ b/libraries/DataFlash/DataFlash.cpp @@ -47,6 +47,10 @@ const AP_Param::GroupInfo DataFlash_Class::var_info[] = { void DataFlash_Class::Init(const struct LogStructure *structures, uint8_t num_types) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + validate_structures(structures, num_types); + dump_structures(structures, num_types); +#endif if (_next_backend == DATAFLASH_MAX_BACKENDS) { AP_HAL::panic("Too many backends"); return; @@ -100,6 +104,128 @@ void DataFlash_Class::Init(const struct LogStructure *structures, uint8_t num_ty } } +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include + +#define DEBUG_LOG_STRUCTURES 0 + +extern const AP_HAL::HAL& hal; +#define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0) + +/// return the number of commas present in string +static uint8_t count_commas(const char *string) +{ + uint8_t ret = 0; + for (uint8_t i=0; iname); + char label[32] = { }; + uint8_t labeloffset = 0; + int8_t fieldnum = 0; + for (uint8_t j=0; jlabels); j++) { + char labelchar = structure->labels[j]; + if (labelchar == '\0') { + break; + } + if (labelchar == ',') { + dump_structure_field(structure, label, fieldnum); + fieldnum++; + labeloffset = 0; + memset(label, '\0', 32); + } else { + label[labeloffset++] = labelchar; + } + } + dump_structure_field(structure, label, fieldnum); + ::fprintf(stderr, "\n"); // just add a CR to the output + } +#endif +} + +void DataFlash_Class::validate_structures(const struct LogStructure *structures, const uint8_t num_types) +{ + Debug("Validating structures"); + bool passed = true; + + bool seen_ids[256] = { }; + for (uint16_t i=0; imsg_type, logstructure->name); +#endif + + // names must be null-terminated + if (logstructure->name[4] != '\0') { + Debug("Message name not NULL-terminated"); + passed = false; + } + + // ensure each message ID is only used once + if (seen_ids[logstructure->msg_type]) { + Debug("ID %d used twice (LogStructure offset=%d)", logstructure->msg_type, i); + passed = false; + } + seen_ids[logstructure->msg_type] = true; + + // ensure we have enough labels to cover columns + uint8_t fieldcount = strlen(logstructure->format); + uint8_t labelcount = count_commas(logstructure->labels)+1; + if (fieldcount != labelcount) { + Debug("fieldcount=%u does not match labelcount=%u", + fieldcount, labelcount); + passed = false; + } + + // check that the structure is of an appropriate length to take fields + const int16_t msg_len = Log_Write_calc_msg_len(logstructure->format); + if (msg_len != logstructure->msg_len) { + Debug("Calculated message length for (%s) based on format field (%s) does not match structure size (%d != %u)", logstructure->name, logstructure->format, msg_len, logstructure->msg_len); + passed = false; + } + } + if (!passed) { + Debug("Log structures are invalid"); + abort(); + } +} + +#else + +void DataFlash_Class::dump_structure_field(const struct LogStructure *_structure, const char *label, const uint8_t fieldnum) +{ +} + +void DataFlash_Class::dump_structures(const struct LogStructure *structures, const uint8_t num_types) +{ +} + +void DataFlash_Class::validate_structures(const struct LogStructure *structures, const uint8_t num_types) +{ + return; +} + +#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL + const struct LogStructure *DataFlash_Class::structure(uint16_t num) const { return &_structures[num]; diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index f6a1dcde39..5c114a8ee1 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -270,4 +270,8 @@ private: private: static DataFlash_Class *_instance; + + void validate_structures(const struct LogStructure *structures, const uint8_t num_types); + void dump_structure_field(const struct LogStructure *structure, const char *label, const uint8_t fieldnum); + void dump_structures(const struct LogStructure *structures, const uint8_t num_types); };