diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/survive_cal.c | 5 | ||||
-rw-r--r-- | src/survive_config.c | 243 | ||||
-rw-r--r-- | src/survive_config.h | 41 |
3 files changed, 170 insertions, 119 deletions
diff --git a/src/survive_cal.c b/src/survive_cal.c index 2ed1986..f372309 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -15,6 +15,8 @@ #include <sys/stat.h> #include <sys/types.h> +#include "survive_config.h" + #define PTS_BEFORE_COMMON 32 #define NEEDED_COMMON_POINTS 10 #define NEEDED_TIMES_OF_COMMON 5 @@ -49,6 +51,9 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) b->fcalgibmag[0] = v6.fcal_0_gibmag; b->fcalgibmag[1] = v6.fcal_1_gibmag; b->OOTXSet = 1; + + config_set_lighthouse(b,id); + config_save("config.json"); } int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length ) diff --git a/src/survive_config.c b/src/survive_config.c index ed3f6cd..dfbd0e2 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -1,173 +1,210 @@ +// (C) 2017 <>< Joshua Allen, Under MIT/x11 License. #include <string.h> #include <assert.h> #include "survive_config.h" +#include <json_helpers.h> #define MAX_CONFIG_ENTRIES 100 +#define MAX_LIGHTHOUSES 2 -config_val config_values[MAX_CONFIG_ENTRIES]; -static uint16_t used_entries = 0; -static FILE *config_file = NULL; -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; - } +config_group global_config_values; +config_group lh_config[MAX_LIGHTHOUSES]; //lighthouse configs - used_entries = 0; -} +//static uint16_t used_entries = 0; -void write_float(char* tag, FLT x) { - fprintf(config_file, "\"%s\":\"%f\"\n", tag, x); -} +static FILE *config_file = NULL; +const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* values, uint8_t count); + +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; -void write_float_a(char* tag, FLT *x, uint8_t count) { - uint8_t i = 0; - char idx[4]; for (i=0;i<count;++i) { - sprintf(idx,"%d",i); - fprintf(config_file, "\"%s%s\":\"%f\"\n", tag,idx, x); + cg->config_entries[i].data = NULL; + cg->config_entries[i].tag = NULL; + cg->config_entries[i].type = CONFIG_UNKNOWN; + cg->config_entries[i].elements = 0; } - 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 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_open(const char* path, const char* mode) { - config_file = fopen(path, mode); +void config_load(const char* path) { + config_file = fopen(path, "r"); } void config_close() { fclose(config_file); } -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(struct 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,"position", bsd->Position, 3); + config_set_float_a(cg,"quaternion", bsd->Quaternion, 4); + 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); + assert(dest!=NULL); + + if (*dest == NULL) { + *dest = (char*)malloc(len); } else { - dest = (char*)realloc(dest, len); + *dest = (char*)realloc(*dest, len); } - strcpy(dest,src); + 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; - } - } - assert(used_entries<MAX_CONFIG_ENTRIES); +FLT config_read_float(config_group *cg, const char *tag, const FLT def) { + config_entry *cv = find_config_entry(cg, tag); - i = used_entries++; - sstrcpy(config_values[i].tag, tag); - config_values[i].numeric.f = def; + if (cv != NULL) return cv->numeric.f; - return config_values[i].numeric.f; + config_set_float(cg, tag, def); } -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 = cg->config_entries + cg->used_entries; + assert(cg->used_entries < cg->max_entries); + cg->used_entries++; + return cv; +} - assert(used_entries<MAX_CONFIG_ENTRIES); +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); + + sstrcpy(&(cv->tag), tag); + cv->numeric.i = value; + cv->type = CONFIG_UINT32; + + return value; +} - assert(used_entries<MAX_CONFIG_ENTRIES); +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); - i = used_entries++; - sstrcpy(config_values[i].tag, tag); - config_values[i].numeric.i = value; + 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); + + if (cv->data == NULL) { + cv->data = (char*)malloc(sizeof(FLT)*count); + } + else { + cv->data = (char*)realloc(cv->data, sizeof(FLT)*count); + } + 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) { + json_write_double_array(f,tag,v,count); +} + +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); } diff --git a/src/survive_config.h b/src/survive_config.h index 24762cd..14e2fc6 100644 --- a/src/survive_config.h +++ b/src/survive_config.h @@ -10,14 +10,11 @@ typedef enum { CONFIG_UNKNOWN = 0, CONFIG_FLOAT = 1, CONFIG_UINT32 = 2, - CONFIG_STRING = 3 + CONFIG_STRING = 3, + CONFIG_FLOAT_ARRAY = 4, } cval_type; -/* -typedef union { - uint32_t i; - FLT f; - } Numeric; -*/ + + typedef struct { char *tag; cval_type type; @@ -25,21 +22,33 @@ typedef struct { uint32_t i; FLT f; } numeric; - char *str; -} config_val; + char *data; + uint32_t elements; +} config_entry; + +typedef struct { + config_entry *config_entries; + uint16_t used_entries; + uint16_t max_entries; +} config_group; + +extern config_group global_config_values; +extern config_group lh_config[2]; //lighthouse configs +void config_init(); void config_open(const char* path, const char* mode); void config_close(); -void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length); +//void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length); +void config_set_lighthouse(struct BaseStationData* bsd, uint8_t idx); void config_save(const char* path); -const FLT config_set_float(const char *tag, const FLT value); -const uint32_t config_set_uint32(const char *tag, const uint32_t value); -const char* config_set_str(const char *tag, const char* value); -FLT config_read_float(const char *tag, const FLT value, const FLT def); +const FLT config_set_float(config_group *cg, const char *tag, const FLT value); +const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32_t value); +const char* config_set_str(config_group *cg, const char *tag, const char* value); -uint32_t config_read_uint32(const char *tag, const uint32_t value, const uint32_t def); -const char* config_read_str(const char *tag, const char *value, const char *def_str); +FLT config_read_float(config_group *cg, const char *tag, const FLT def); +uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def); +const char* config_read_str(config_group *cg, const char *tag, const char *def); #endif
\ No newline at end of file |