diff options
author | mwturvey <michael.w.turvey@intel.com> | 2017-03-08 10:47:27 -0700 |
---|---|---|
committer | mwturvey <michael.w.turvey@intel.com> | 2017-03-08 10:47:27 -0700 |
commit | a5430b24d41d1bb795db14036b35c39c7a9accd4 (patch) | |
tree | 90120d0a3d529aca81d77fe825229f8a6d641bc6 /src/survive_config.c | |
parent | 321749a4b8e3e8b3f4b03863200fbf86e36a0bbe (diff) | |
parent | c65498054c77192b2a12fdb5ef44439a14110292 (diff) | |
download | libsurvive-a5430b24d41d1bb795db14036b35c39c7a9accd4.tar.gz libsurvive-a5430b24d41d1bb795db14036b35c39c7a9accd4.tar.bz2 |
Merge branch 'master' of https://github.com/cnlohr/libsurvive
Diffstat (limited to 'src/survive_config.c')
-rw-r--r-- | src/survive_config.c | 393 |
1 files changed, 289 insertions, 104 deletions
diff --git a/src/survive_config.c b/src/survive_config.c index ed3f6cd..352a15b 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -1,174 +1,359 @@ +// (C) 2017 <>< Joshua Allen, Under MIT/x11 License. #include <string.h> #include <assert.h> #include "survive_config.h" +#include <json_helpers.h> + +#include <errno.h> #define MAX_CONFIG_ENTRIES 100 +#define MAX_LIGHTHOUSES 2 -config_val config_values[MAX_CONFIG_ENTRIES]; -static uint16_t used_entries = 0; +config_group global_config_values; +config_group lh_config[MAX_LIGHTHOUSES]; //lighthouse configs -static FILE *config_file = NULL; +//static uint16_t used_entries = 0; -void config_init() { - uint16_t i = 0; - for (i=0;i<MAX_CONFIG_ENTRIES;++i) { - config_values[i].str = NULL; - config_values[i].tag = NULL; - } +//static FILE *config_file = NULL; +const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* values, uint8_t count); - used_entries = 0; +void init_config_entry(config_entry* ce) { + ce->data = NULL; + ce->tag = NULL; + ce->type = CONFIG_UNKNOWN; + ce->elements = 0; } -void write_float(char* tag, FLT x) { - fprintf(config_file, "\"%s\":\"%f\"\n", tag, x); +void destroy_config_entry(config_entry* ce) { + if (ce->tag!=NULL) free(ce->tag); + if (ce->data!=NULL) free(ce->data); } -void write_float_a(char* tag, FLT *x, uint8_t count) { - uint8_t i = 0; - char idx[4]; +void init_config_group(config_group *cg, uint16_t count) { + uint16_t i = 0; + cg->config_entries = malloc(count*sizeof(config_entry)); + cg->used_entries = 0; + cg->max_entries = count; + for (i=0;i<count;++i) { - sprintf(idx,"%d",i); - fprintf(config_file, "\"%s%s\":\"%f\"\n", tag,idx, x); + init_config_entry(cg->config_entries+i); } - fprintf(config_file, "\"%s\":\"%f\"\n", tag, x); } -void write_uint32(char* tag, uint32_t x) { - fprintf(config_file, "\"%s\":\"%d\"\n", tag, x); -} +void resize_config_group(config_group *cg, uint16_t count) { + uint16_t i = 0; + + if (count > cg->max_entries) { + config_entry* ptr = realloc(cg->config_entries, sizeof(config_entry)*count); + assert(ptr!=NULL); + + cg->config_entries = ptr; -void config_open(const char* path, const char* mode) { - config_file = fopen(path, mode); + for (i=cg->max_entries;i<count;++i) { + init_config_entry(cg->config_entries+i); + } + + cg->max_entries = count; + } } -void config_close() { - fclose(config_file); +void config_init() { + uint16_t i = 0; + init_config_group(&global_config_values, MAX_CONFIG_ENTRIES); + for(i=0;i<MAX_LIGHTHOUSES;i++) { + init_config_group(lh_config+i, 9); + } } -void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length) { - uint8_t i = 0; - for (i=0;i<length; ++i) { - write_uint32("id", bsd[i].BaseStationID); - write_float_a("position", bsd[i].Position, 3); - write_float_a("quaternion", bsd[i].Quaternion, 4); - write_float_a("quaternion", bsd[i].Quaternion, 4); - write_float_a("fcalphase", bsd[i].fcalphase, 2); - write_float_a("fcaltilt", bsd[i].fcaltilt,2); - write_float_a("fcalcurve", bsd[i].fcalcurve,2); - write_float_a("fcalgibpha", bsd[i].fcalgibpha,2); - write_float_a("fcalgibmag", bsd[i].fcalgibmag,2); - } +void config_set_lighthouse(BaseStationData* bsd, uint8_t idx) { + config_group *cg = lh_config+idx; + config_set_uint32(cg,"index", idx); + config_set_uint32(cg,"id", bsd->BaseStationID); + config_set_float_a(cg,"pose", &bsd->Pose.Pos[0], 7); + config_set_float_a(cg,"fcalphase", bsd->fcalphase, 2); + config_set_float_a(cg,"fcaltilt", bsd->fcaltilt,2); + config_set_float_a(cg,"fcalcurve", bsd->fcalcurve,2); + config_set_float_a(cg,"fcalgibpha", bsd->fcalgibpha,2); + config_set_float_a(cg,"fcalgibmag", bsd->fcalgibmag,2); } -void sstrcpy(char* dest, const char *src) { +void sstrcpy(char** dest, const char *src) { uint32_t len = strlen(src)+1; - if (dest == NULL) { - dest = (char*)malloc(len); - } else { - dest = (char*)realloc(dest, len); - } - strcpy(dest,src); + assert(dest!=NULL); + + char* ptr = (char*)realloc(*dest, len); //acts like malloc if dest==NULL + assert(ptr!=NULL); + *dest = ptr; + + strcpy(*dest,src); } -const char* config_read_str(const char *tag, const char *value, const char *def_str) { +config_entry* find_config_entry(config_group *cg, const char *tag) { uint16_t i = 0; - for (i=0;i<used_entries;++i) { - if ( strcmp(config_values[i].tag, tag) == 0 ) { - return config_values[i].str; + for (i=0;i < cg->used_entries;++i) { + if ( strcmp(cg->config_entries[i].tag, tag) == 0 ) { + return cg->config_entries+i; } } - assert(used_entries<MAX_CONFIG_ENTRIES); + return NULL; +} + +const char* config_read_str(config_group *cg, const char *tag, const char *def) { + config_entry *cv = find_config_entry(cg, tag); - i = used_entries++; - sstrcpy(config_values[i].tag, tag); - sstrcpy(config_values[i].str, def_str); + if (cv != NULL) return cv->data; - return config_values[i].str; + return config_set_str(cg,tag,def); } -uint32_t config_read_uint32(const char *tag, const uint32_t value, const uint32_t def) { - uint16_t i = 0; - for (i=0;i<used_entries;++i) { - if ( strcmp(config_values[i].tag, tag) == 0 ) { - return config_values[i].numeric.i; - } - } - assert(used_entries<MAX_CONFIG_ENTRIES); +uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def) { + config_entry *cv = find_config_entry(cg, tag); - i = used_entries++; - sstrcpy(config_values[i].tag, tag); - config_values[i].numeric.i = def; + if (cv != NULL) return cv->numeric.i; - return config_values[i].numeric.i; + return config_set_uint32(cg, tag, def); } -FLT config_read_float(const char *tag, const FLT value, const FLT def) { - uint16_t i = 0; - for (i=0;i<used_entries;++i) { - if ( strcmp(config_values[i].tag, tag) == 0 ) { - return config_values[i].numeric.f; - } +FLT config_read_float(config_group *cg, const char *tag, const FLT def) { + config_entry *cv = find_config_entry(cg, tag); + + if (cv != NULL) return cv->numeric.f; + + return config_set_float(cg, tag, def); +} + +uint16_t config_read_float_array(config_group *cg, const char *tag, FLT** values, const FLT* def, uint16_t count) { + config_entry *cv = find_config_entry(cg, tag); + + if (cv != NULL) { + *values = (FLT*)cv->data; + return cv->elements; } - assert(used_entries<MAX_CONFIG_ENTRIES); - i = used_entries++; - sstrcpy(config_values[i].tag, tag); - config_values[i].numeric.f = def; + if (def == NULL) return 0; - return config_values[i].numeric.f; + config_set_float_a(cg, tag, def, count); + *values = def; + return count; } -const char* config_set_str(const char *tag, const char* value) { - uint16_t i = 0; +config_entry* next_unused_entry(config_group *cg) { + config_entry *cv = NULL; +// assert(cg->used_entries < cg->max_entries); + + if (cg->used_entries >= cg->max_entries) resize_config_group(cg, cg->max_entries + 10); + + cv = cg->config_entries + cg->used_entries; - assert(used_entries<MAX_CONFIG_ENTRIES); + cg->used_entries++; + return cv; +} + +const char* config_set_str(config_group *cg, const char *tag, const char* value) { + config_entry *cv = find_config_entry(cg, tag); + if (cv == NULL) cv = next_unused_entry(cg); - i = used_entries++; - sstrcpy(config_values[i].tag, tag); - sstrcpy(config_values[i].str, value); + sstrcpy(&(cv->tag), tag); + sstrcpy(&(cv->data), value); + cv->type = CONFIG_STRING; return value; } -const uint32_t config_set_uint32(const char *tag, const uint32_t value) { - uint16_t i = 0; +const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32_t value) { + config_entry *cv = find_config_entry(cg, tag); + if (cv == NULL) cv = next_unused_entry(cg); - assert(used_entries<MAX_CONFIG_ENTRIES); + sstrcpy(&(cv->tag), tag); + cv->numeric.i = value; + cv->type = CONFIG_UINT32; - i = used_entries++; - sstrcpy(config_values[i].tag, tag); - config_values[i].numeric.i = value; + return value; +} + +const FLT config_set_float(config_group *cg, const char *tag, const FLT value) { + config_entry *cv = find_config_entry(cg, tag); + if (cv == NULL) cv = next_unused_entry(cg); + + sstrcpy(&(cv->tag), tag); + cv->numeric.f = value; + cv->type = CONFIG_FLOAT; return value; } -const FLT config_set_float(const char *tag, const FLT value) { +const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* values, uint8_t count) { + config_entry *cv = find_config_entry(cg, tag); + if (cv == NULL) cv = next_unused_entry(cg); + + sstrcpy(&(cv->tag), tag); + + char* ptr = (char*)realloc(cv->data, sizeof(FLT)*count); + assert(ptr!=NULL); + cv->data = ptr; + + printf("float array\n"); + + memcpy(cv->data,values,sizeof(FLT)*count); + cv->type = CONFIG_FLOAT_ARRAY; + cv->elements = count; + + return values; +} + +void _json_write_float_array(FILE* f, const char* tag, FLT* v, uint8_t count) { + #ifdef USE_DOUBLE + json_write_double_array(f,tag,v,count); + #else + json_write_float_array(f,tag,v,count); + #endif +} + +void write_config_group(FILE* f, config_group *cg, char *tag) { uint16_t i = 0; - assert(used_entries<MAX_CONFIG_ENTRIES); + if (tag != NULL) { + fprintf(f, "\"%s\":{\n", tag); + } - i = used_entries++; - sstrcpy(config_values[i].tag, tag); - config_values[i].numeric.f = value; + for (i=0;i < cg->used_entries;++i) { + if (cg->config_entries[i].type == CONFIG_FLOAT) { + json_write_float(f, cg->config_entries[i].tag, cg->config_entries[i].numeric.f); + } else if (cg->config_entries[i].type == CONFIG_UINT32) { + json_write_uint32(f, cg->config_entries[i].tag, cg->config_entries[i].numeric.i); + } else if (cg->config_entries[i].type == CONFIG_STRING) { + json_write_str(f, cg->config_entries[i].tag, cg->config_entries[i].data); + } else if (cg->config_entries[i].type == CONFIG_FLOAT_ARRAY) { + _json_write_float_array(f, cg->config_entries[i].tag, (FLT*)cg->config_entries[i].data, cg->config_entries[i].elements); + } + if ((i+1) < cg->used_entries) fprintf(f,","); + fprintf(f,"\n"); + }; - return value; + if (tag != NULL) { + fprintf(f,"}\n"); + } } + void config_save(const char* path) { uint16_t i = 0; FILE* f = fopen(path, "w"); - for (i=0;i<=used_entries;++i) { - if (config_values[i].type == CONFIG_FLOAT) { - fprintf(f, "\"%s\":\"%F\"\n", config_values[i].tag, config_values[i].numeric.f); - } else if (config_values[i].type == CONFIG_UINT32) { - fprintf(f, "\"%s\":\"%d\"\n", config_values[i].tag, config_values[i].numeric.i); - } else if (config_values[i].type == CONFIG_STRING) { - fprintf(f, "\"%s\":\"%s\"\n", config_values[i].tag, config_values[i].str); - } - }; + write_config_group(f,&global_config_values, NULL); + write_config_group(f,lh_config, "lighthouse0"); + write_config_group(f,lh_config+1, "lighthouse1"); fclose(f); } +void print_json_value(char* tag, char** values, uint16_t count) { + uint16_t i = 0; + for (i=0;i<count; ++i) { + printf("%s:%s \n", tag, values[i]); + } +} + +config_group* cg_stack[10]; //handle 10 nested objects deep +uint8_t cg_stack_head = 0; + +void handle_config_group(char* tag) { + cg_stack_head++; + if (strcmp("lighthouse0",tag) == 0) { + cg_stack[cg_stack_head] = lh_config; + } else if (strcmp("lighthouse1",tag) == 0) { + cg_stack[cg_stack_head] = lh_config+1; + } else { + cg_stack[cg_stack_head] = &global_config_values; + } +} + +void pop_config_group() { + cg_stack_head--; +} + + +int parse_floats(char* tag, char** values, uint16_t count) { + uint16_t i = 0; + FLT f[count]; + char* end = NULL; + config_group* cg = cg_stack[cg_stack_head]; + + for(i=0;i<count;++i) { + + #ifdef USE_DOUBLE + f[i] = strtod(values[i], &end); + #else + f[i] = strtof(values[i], &end); + #endif + +// if (values[i] == end) return 0; //not a float + if (*end != '\0') return 0; //not an integer + } + + if (count>1) { + config_set_float_a(cg, tag, f, count); + } + else { + config_set_float(cg, tag, f[0]); + } + + return 1; +} + +int parse_uint32(char* tag, char** values, uint16_t count) { + uint16_t i = 0; + uint32_t l[count]; + char* end = NULL; + config_group* cg = cg_stack[cg_stack_head]; + +/* + //look for non numeric values + for(end=values[0];*end!='\0';++end) { + if ((*end<48) || (*end>57)) return 0; + } + + end=NULL; +*/ + for(i=0;i<count;++i) { + l[i] = strtol(values[i], &end, 10); +// if (values[i] == end) return 0; //not an integer + if (*end != '\0') return 0; //not an integer + } + +// if (count>1) +// config_set_uint32_array(cg, tag, f, count); +// else + config_set_uint32(cg, tag, l[0]); + + return 1; +} + +void handle_tag_value(char* tag, char** values, uint16_t count) { + print_json_value(tag,values,count); + config_group* cg = cg_stack[cg_stack_head]; + + if (parse_uint32(tag,values,count) > 0) return; //parse integers first, stricter rules + + if (parse_floats(tag,values,count) > 0) return; + + //should probably also handle string arrays + config_set_str(cg,tag,values[0]); +// else if (count>1) config_set_str +} + +void config_read(const char* path) { + json_begin_object = handle_config_group; + json_end_object = pop_config_group; + json_tag_value = handle_tag_value; + + cg_stack[0] = &global_config_values; + + json_load_file(path); + + json_begin_object = NULL; + json_end_object = NULL; + json_tag_value = NULL; +} + |