diff --git a/stm-firmware/config-parser/config-parser.c b/stm-firmware/config-parser/config-parser.c index 0e67386..db15072 100644 --- a/stm-firmware/config-parser/config-parser.c +++ b/stm-firmware/config-parser/config-parser.c @@ -71,6 +71,7 @@ static int parse_value(struct config_parser_entry *entry, char *value_start_tok if (endptr == value_start_token) return -1; entry->type = CONFIG_PARSER_TYPE_FLOAT; + goto exit; } if (value_start_token[0] != '-') { @@ -81,6 +82,7 @@ static int parse_value(struct config_parser_entry *entry, char *value_start_tok return -1; } entry->type = CONFIG_PARSER_TYPE_UINT; + goto exit; } else { /* Try parsing as int */ entry->value.int_val = strtod(value_start_token, &endptr); @@ -90,6 +92,7 @@ static int parse_value(struct config_parser_entry *entry, char *value_start_tok entry->type = CONFIG_PARSER_TYPE_INT; } +exit: return 0; } @@ -128,7 +131,7 @@ enum config_parser_ret config_parser_get_line(config_parser_handle_t handle, str } break; case 2: /* VALUE */ - if (!parse_value(entry, token)) + if (parse_value(entry, token)) return CONFIG_PARSER_LINE_MALFORM; break; default: @@ -136,7 +139,7 @@ enum config_parser_ret config_parser_get_line(config_parser_handle_t handle, str } token_round++; - strtok(NULL, token_delim); + token = strtok(NULL, token_delim); } return CONFIG_PARSER_OK; diff --git a/stm-firmware/settings/settings-sd-card.c b/stm-firmware/settings/settings-sd-card.c index 1934767..3f841b8 100644 --- a/stm-firmware/settings/settings-sd-card.c +++ b/stm-firmware/settings/settings-sd-card.c @@ -25,6 +25,9 @@ #include #include #include +#include + +static char workbuff[256]; static void get_controller_folder_path(char *path, size_t size) { @@ -46,7 +49,7 @@ static void get_controller_settings_path(char *path, size_t size, const char *se char folder[48]; get_controller_folder_path(folder, sizeof(folder)); - snprintf(path, size, "%s/%s.dat", folder, setting); + snprintf(path, size, "%s/%s.conf", folder, setting); } /** @@ -79,104 +82,79 @@ static int create_controller_folder(void) return ret; } -static int read_settings_file_float(const char *path, float *value) -{ - FRESULT res; - FIL file; - int ret = 0; - char buff[32]; - UINT read_count; - if (!value) - return -1002; - - res = f_open(&file, path, FA_READ); - if (res == FR_OK) { - memset(buff, 0, sizeof(buff)); - res = f_read(&file, buff, sizeof(buff)-1, &read_count); - if (res != FR_OK) { - ret = -1; - goto close_file; - } - *value = strtof(buff, NULL); -close_file: - f_close(&file); - } else { - ret = -2; - } - - return ret; -} int sd_card_settings_save_calibration(float sens_deviation, float offset, bool active) { - int status; - char path[128]; - char buff[64]; - UINT bw; + char path[200]; + FRESULT res = FR_OK; + int ret; FIL file; - FRESULT res; - int ret = 0; - status = create_controller_folder(); - if (status < 0) + get_controller_settings_path(path, sizeof(path), "calibration"); + + if (create_controller_folder() < 0) return -2; - get_controller_settings_path(path, sizeof(path), "offset"); - if (active) { - res = f_open(&file, path, FA_CREATE_ALWAYS | FA_WRITE); - if (res != FR_OK) { - ret = -2; - goto exit_offset; - } - - status = snprintf(buff, sizeof(buff), "%f\n", offset); - f_write(&file, buff, status, &bw); - f_close(&file); - } else { - f_unlink(path); + if (!active) { + res = f_unlink(path); + goto check_fresult; } -exit_offset: - get_controller_settings_path(path, sizeof(path), "sens"); - if (active) { - res = f_open(&file, path, FA_CREATE_ALWAYS | FA_WRITE); - if (res != FR_OK) { - ret = -2; - goto exit_sens; - } - status = snprintf(buff, sizeof(buff), "%f\n", sens_deviation); - f_write(&file, buff, status, &bw); - f_close(&file); - } else { - f_unlink(path); - } -exit_sens: + res = f_open(&file, path, FA_CREATE_ALWAYS | FA_WRITE); + if (res != FR_OK) + goto check_fresult; + + snprintf(path, sizeof(path), "offset = %f\nsensitivity = %f\n", offset, sens_deviation); + ret = f_puts(path, &file); + if (ret < 0) + goto close_file; + + ret = 0; + +close_file: + res = f_close(&file); +check_fresult: + if (res != FR_OK) + return -2; return ret; } int sd_card_settings_try_load_calibration(float *sens_deviation, float *offset) { char path[128]; - int status; - int ret = 0; + int status = -1; + struct config_parser parser; + config_parser_handle_t p; + enum config_parser_ret res; + struct config_parser_entry entry; + bool sens_loaded = false; + bool offset_loaded = false; if (!sens_deviation || !offset) return -1000; - get_controller_settings_path(path, sizeof(path), "offset"); - status = read_settings_file_float(path, offset); - if (status) { - ret = status; - goto exit; - } + get_controller_settings_path(path, sizeof(path), "calibration"); + p = config_parser_open_file(&parser, false, path, workbuff, sizeof(workbuff)); + status = 0; + do { + res = config_parser_get_line(p, &entry); + if (res == CONFIG_PARSER_OK) { + if (!strcmp(entry.name, "offset") && entry.type == CONFIG_PARSER_TYPE_FLOAT) { + offset_loaded = true; + *offset = entry.value.float_val; + } else if (!strcmp(entry.name, "sensitivity") && entry.type == CONFIG_PARSER_TYPE_FLOAT) { + sens_loaded = true; + *sens_deviation = entry.value.float_val; + } + } - get_controller_settings_path(path, sizeof(path), "sens"); - status = read_settings_file_float(path, sens_deviation); - if (status) { - ret = status; - goto exit; - } -exit: - return ret; + } while (res != CONFIG_PARSER_END_REACHED && + res != CONFIG_PARSER_GENERIC_ERR && + res != CONFIG_PARSER_IOERR); + + if (sens_loaded && offset_loaded) + status = 0; + + return status; }