DataFlash_Class::log_write_fmt *DataFlash_Class::msg_fmt_for_name(const char *name, const char *labels, const char *fmt)
{
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 f;
}
}
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) {
free(f);
return nullptr;
}
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) {
free(f);
return nullptr;
}
f->msg_len = tmp;
// add to front of list
f->next = log_write_fmts;
log_write_fmts = f;
return f;
}
AP_Logger::log_write_fmt *AP_Logger::msg_fmt_for_name(const char *name, const char *labels, const char *units, const char *mults, const char *fmt)
{
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:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
assert_same_fmt_for_name(f, name, labels, units, mults, fmt);
#endif
return f;
}
}
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) {
free(f);
return nullptr;
}
f->msg_type = msg_type;
f->name = name;
f->fmt = fmt;
f->labels = labels;
f->units = units;
f->mults = mults;
int16_t tmp = Write_calc_msg_len(fmt);
if (tmp == -1) {
free(f);
return nullptr;
}
f->msg_len = tmp;
// add to front of list
f->next = log_write_fmts;
log_write_fmts = f;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
char ls_name[LS_NAME_SIZE] = {};
char ls_format[LS_FORMAT_SIZE] = {};
char ls_labels[LS_LABELS_SIZE] = {};
char ls_units[LS_UNITS_SIZE] = {};
char ls_multipliers[LS_MULTIPLIERS_SIZE] = {};
struct LogStructure ls = {
f->msg_type,
f->msg_len,
ls_name,
ls_format,
ls_labels,
ls_units,
ls_multipliers
};
memcpy((char*)ls_name, f->name, MIN(sizeof(ls_name), strlen(f->name)));
memcpy((char*)ls_format, f->fmt, MIN(sizeof(ls_format), strlen(f->fmt)));
memcpy((char*)ls_labels, f->labels, MIN(sizeof(ls_labels), strlen(f->labels)));
if (f->units != nullptr) {
memcpy((char*)ls_units, f->units, MIN(sizeof(ls_units), strlen(f->units)));
} else {
memset((char*)ls_units, '?', MIN(sizeof(ls_format), strlen(f->fmt)));
}
if (f->mults != nullptr) {
memcpy((char*)ls_multipliers, f->mults, MIN(sizeof(ls_multipliers), strlen(f->mults)));
} else {
memset((char*)ls_multipliers, '?', MIN(sizeof(ls_format), strlen(f->fmt)));
}
validate_structure(&ls, (int16_t)-1);
#endif
return f;
}
