Merge branch 'master' of https://github.com/cnlohr/libsurvive

3 files changed, 221 insertions, 2 deletions

diff --git a/Makefile b/Makefile
index 9940260..693f6bc 100644
--- a/Makefile
+++ b/Makefile
@@ -1,6 +1,6 @@
 all : lib data_recorder test calibrate calibrate_client
 
-CFLAGS:=-Iinclude -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE
+CFLAGS:=-Iinclude -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99
 LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lm -flto -g
 
 
@@ -23,7 +23,7 @@ calibrate_client :  calibrate_client.c lib/libsurvive.so redist/os_generic.c red
 lib:
 	mkdir lib
 
-lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o src/ootx_decoder.o redist/linmath.o $(DEBUGSTUFF) $(CALS)
+lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o src/ootx_decoder.o redist/linmath.o src/survive_config.o $(DEBUGSTUFF) $(CALS)
 	gcc -o $@ $^ $(LDFLAGS) -shared
 
 clean :
diff --git a/src/survive_config.c b/src/survive_config.c
new file mode 100644
index 0000000..ed3f6cd
--- /dev/null
+++ b/src/survive_config.c
@@ -0,0 +1,174 @@
+#include <string.h>
+#include <assert.h>
+#include "survive_config.h"
+
+#define MAX_CONFIG_ENTRIES 100
+
+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;
+	}
+
+	used_entries = 0;
+}
+
+void write_float(char* tag, FLT x) {
+	fprintf(config_file, "\"%s\":\"%f\"\n", tag, x);
+}
+
+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);
+	}
+	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_open(const char* path, const char* mode) {
+	config_file = fopen(path, mode);
+}
+
+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 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);
+}
+
+const char* config_read_str(const char *tag, const char *value, const char *def_str) {
+	uint16_t i = 0;
+	for (i=0;i<used_entries;++i) {
+		if ( strcmp(config_values[i].tag, tag) == 0 ) {
+			return config_values[i].str;
+		}
+	}
+	assert(used_entries<MAX_CONFIG_ENTRIES);
+
+	i = used_entries++;
+	sstrcpy(config_values[i].tag, tag);
+	sstrcpy(config_values[i].str, def_str);
+
+	return config_values[i].str;
+}
+
+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);
+
+	i = used_entries++;
+	sstrcpy(config_values[i].tag, tag);
+	config_values[i].numeric.i = def;
+
+	return config_values[i].numeric.i;
+}
+
+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);
+
+	i = used_entries++;
+	sstrcpy(config_values[i].tag, tag);
+	config_values[i].numeric.f = def;
+
+	return config_values[i].numeric.f;
+}
+
+const char* config_set_str(const char *tag, const char* value) {
+	uint16_t i = 0;
+
+	assert(used_entries<MAX_CONFIG_ENTRIES);
+
+	i = used_entries++;
+	sstrcpy(config_values[i].tag, tag);
+	sstrcpy(config_values[i].str, value);
+
+	return value;
+}
+
+const uint32_t config_set_uint32(const char *tag, const uint32_t value) {
+	uint16_t i = 0;
+
+	assert(used_entries<MAX_CONFIG_ENTRIES);
+
+	i = used_entries++;
+	sstrcpy(config_values[i].tag, tag);
+	config_values[i].numeric.i = value;
+
+	return value;
+}
+
+const FLT config_set_float(const char *tag, const FLT value) {
+	uint16_t i = 0;
+
+	assert(used_entries<MAX_CONFIG_ENTRIES);
+
+	i = used_entries++;
+	sstrcpy(config_values[i].tag, tag);
+	config_values[i].numeric.f = value;
+
+	return value;
+}
+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);
+		}
+	};
+
+	fclose(f);
+}
+
diff --git a/src/survive_config.h b/src/survive_config.h
new file mode 100644
index 0000000..24762cd
--- /dev/null
+++ b/src/survive_config.h
@@ -0,0 +1,45 @@
+// (C) 2017 <>< Joshua Allen, Under MIT/x11 License.
+
+
+#ifndef _SURVIVE_CONFIG_H
+#define _SURVIVE_CONFIG_H
+
+#include "survive_internal.h"
+
+typedef enum {
+	CONFIG_UNKNOWN = 0,
+	CONFIG_FLOAT = 1,
+	CONFIG_UINT32 = 2,
+	CONFIG_STRING = 3
+} cval_type;
+/*
+typedef union {
+		uint32_t i;
+		FLT f;
+	} Numeric;
+*/
+typedef struct {
+	char *tag;
+	cval_type type;
+	union {
+		uint32_t i;
+		FLT f;
+	} numeric;
+	char *str;
+} config_val;
+
+
+void config_open(const char* path, const char* mode);
+void config_close();
+void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length);
+
+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);
+
+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);
+
+#endif
\ No newline at end of file