Merge remote-tracking branch 'remotes/upstream/master' into Octavios_Algorithm

Conflicts:
	redist/linmath.c

5 files changed, 125 insertions, 29 deletions

diff --git a/src/poser_dummy.c b/src/poser_dummy.c
new file mode 100644
index 0000000..67f8edb
--- /dev/null
+++ b/src/poser_dummy.c
@@ -0,0 +1,52 @@
+#include <survive.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+typedef struct
+{
+	int something;
+	//Stuff
+} DummyData;
+
+int PoserDummy( SurviveObject * so, PoserData * pd )
+{
+	PoserType pt = pd->pt;
+	SurviveContext * ctx = so->ctx;
+	DummyData * dd = so->PoserData;
+
+	if( !dd ) so->PoserData = dd = malloc( sizeof( DummyData ) );
+
+	switch( pt )
+	{
+	case POSERDATA_IMU:
+	{
+		PoserDataIMU * imu = (PoserDataIMU*)pd;
+		//printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] );
+		break;
+	}
+	case POSERDATA_LIGHT:
+	{
+		PoserDataLight * l = (PoserDataLight*)pd;
+		//printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode );
+		break;
+	}
+	case POSERDATA_FULL_SCENE:
+	{
+		PoserDataFullScene * fs = (PoserDataFullScene*)pd;
+		//printf( "Full scene data.\n" );
+		break;
+	}
+	case POSERDATA_DISASSOCIATE:
+	{
+		free( dd );
+		so->PoserData = 0;
+		//printf( "Need to disassociate.\n" );
+		break;
+	}
+	}
+
+}
+
+
+REGISTER_LINKTIME( PoserDummy );
+
diff --git a/src/survive.c b/src/survive.c
index efa5d82..09eb432 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -8,6 +8,8 @@
 #include <string.h>
 #include <zlib.h>
 
+#include "survive_config.h"
+
 static void survivefault( struct SurviveContext * ctx, const char * fault )
 {
 	fprintf( stderr, "Error: %s\n", fault );
@@ -26,6 +28,14 @@ SurviveContext * survive_init( int headless )
 	int i = 0;
 	SurviveContext * ctx = calloc( 1, sizeof( SurviveContext ) );
 
+	ctx->global_config_values = malloc( sizeof(config_group) );
+	ctx->lh_config = malloc( sizeof(config_group) * NUM_LIGHTHOUSES);
+
+	init_config_group(ctx->global_config_values,10);
+	init_config_group(ctx->lh_config,10);
+
+	config_read(ctx, "config.json");
+
 	ctx->faultfunction = survivefault;
 	ctx->notefunction = survivenote;
 
@@ -43,7 +53,7 @@ SurviveContext * survive_init( int headless )
 	}
 
 	i = 0;
-	const char * PreferredPoser = "PoserDummy"; //config_read_str( cg, "DefualtPoser", "PoserDummy" ); /XXX Axlecrusher, can you add config stuff for this?
+	const char * PreferredPoser = config_read_str( ctx->global_config_values, "DefualtPoser", "PoserDummy" );
 	PoserCB PreferredPoserCB = 0;
 	const char * FirstPoser = 0;
 	printf( "Available posers:\n" );
@@ -169,11 +179,20 @@ void survive_close( SurviveContext * ctx )
 		ctx->drivercloses[i]( ctx, ctx->drivers[i] );
 	}
 
+
+	config_save(ctx, "config.json");
+
+	destroy_config_group(ctx->global_config_values);
+	destroy_config_group(ctx->lh_config);
+
 	free( ctx->objs );
 	free( ctx->drivers );
 	free( ctx->driverpolls );
 	free( ctx->drivermagics );
 	free( ctx->drivercloses );
+	free( ctx->global_config_values );
+	free( ctx->lh_config );
+
 	free( ctx );
 }
 
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 2acf51c..06914eb 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -52,7 +52,7 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	b->fcalgibmag[1] = v6.fcal_1_gibmag;
 	b->OOTXSet = 1;
 
-	config_set_lighthouse(b,id);
+	config_set_lighthouse(ctx->lh_config,b,id);
 //	config_save("config.json");
 }
 
diff --git a/src/survive_config.c b/src/survive_config.c
index 352a15b..3f0f199 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -6,11 +6,11 @@
 
 #include <errno.h>
 
-#define MAX_CONFIG_ENTRIES 100
-#define MAX_LIGHTHOUSES 2
+//#define MAX_CONFIG_ENTRIES 100
+//#define MAX_LIGHTHOUSES 2
 
-config_group global_config_values;
-config_group lh_config[MAX_LIGHTHOUSES]; //lighthouse configs
+//config_group global_config_values;
+//config_group lh_config[MAX_LIGHTHOUSES]; //lighthouse configs
 
 //static uint16_t used_entries = 0;
 
@@ -25,21 +25,36 @@ void init_config_entry(config_entry* ce) {
 }
 
 void destroy_config_entry(config_entry* ce) {
-	if (ce->tag!=NULL) free(ce->tag);
-	if (ce->data!=NULL) free(ce->data);
+	if (ce->tag!=NULL) { free(ce->tag); ce->tag=NULL; }
+	if (ce->data!=NULL) { free(ce->data); ce->data=NULL; }
 }
 
 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;
+	cg->config_entries = NULL;
+
+	if (count==0) return;
+
+	cg->config_entries = malloc(count*sizeof(config_entry));
 
 	for (i=0;i<count;++i) {
 		init_config_entry(cg->config_entries+i);
 	}
 }
 
+void destroy_config_group(config_group* cg) {
+	uint16_t i = 0;
+	if (cg->config_entries==NULL) return;
+
+	for (i=0;i<cg->max_entries;++i) {
+		destroy_config_entry(cg->config_entries+i);
+	}
+
+	free(cg->config_entries);
+}
+
 void resize_config_group(config_group *cg, uint16_t count) {
 	uint16_t i = 0;
 
@@ -57,6 +72,7 @@ void resize_config_group(config_group *cg, uint16_t count) {
 	}
 }
 
+/*
 void config_init() {
 	uint16_t i = 0;
 	init_config_group(&global_config_values, MAX_CONFIG_ENTRIES);
@@ -64,9 +80,9 @@ void config_init() {
 		init_config_group(lh_config+i, 9);
 	}
 }
+*/
 
-
-void config_set_lighthouse(BaseStationData* bsd, uint8_t idx) {
+void config_set_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8_t idx) {
 	config_group *cg = lh_config+idx;
 	config_set_uint32(cg,"index", idx);
 	config_set_uint32(cg,"id", bsd->BaseStationID);
@@ -123,7 +139,7 @@ FLT config_read_float(config_group *cg, const char *tag, const FLT def) {
 	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) {
+uint16_t config_read_float_array(config_group *cg, const char *tag, const FLT** values, const FLT* def, uint16_t count) {
 	config_entry *cv = find_config_entry(cg, tag);
 
 	if (cv != NULL) {
@@ -236,14 +252,17 @@ void write_config_group(FILE* f, config_group *cg, char *tag) {
 	}
 }
 
-void config_save(const char* path) {
+//struct SurviveContext;
+SurviveContext* survive_context;
+
+void config_save(SurviveContext* sctx, const char* path) {
 	uint16_t i = 0;
 
 	FILE* f = fopen(path, "w");
 
-	write_config_group(f,&global_config_values, NULL);
-	write_config_group(f,lh_config, "lighthouse0");
-	write_config_group(f,lh_config+1, "lighthouse1");
+	write_config_group(f,sctx->global_config_values, NULL);
+	write_config_group(f,sctx->lh_config, "lighthouse0");
+	write_config_group(f,sctx->lh_config+1, "lighthouse1");
 
 	fclose(f);
 }
@@ -261,11 +280,11 @@ 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;
+		cg_stack[cg_stack_head] = survive_context->lh_config;
 	} else if (strcmp("lighthouse1",tag) == 0) {
-		cg_stack[cg_stack_head] = lh_config+1;
+		cg_stack[cg_stack_head] = survive_context->lh_config+1;
 	} else {
-		cg_stack[cg_stack_head] = &global_config_values;
+		cg_stack[cg_stack_head] = survive_context->global_config_values;
 	}
 }
 
@@ -343,12 +362,14 @@ void handle_tag_value(char* tag, char** values, uint16_t count) {
 //	else if (count>1) config_set_str
 }
 
-void config_read(const char* path) {
+void config_read(SurviveContext* sctx, const char* path) {
+	survive_context = sctx;
+
 	json_begin_object = handle_config_group;
 	json_end_object = pop_config_group;
 	json_tag_value = handle_tag_value;
 
-	cg_stack[0] = &global_config_values;
+	cg_stack[0] = sctx->global_config_values;
 
 	json_load_file(path);
 
diff --git a/src/survive_config.h b/src/survive_config.h
index cd16c76..c8c7762 100644
--- a/src/survive_config.h
+++ b/src/survive_config.h
@@ -26,29 +26,33 @@ typedef struct {
 	uint32_t elements;
 } config_entry;
 
-typedef struct {
+typedef struct config_group {
 	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
+//extern config_group global_config_values;
+//extern config_group lh_config[2]; //lighthouse configs
 
+void init_config_group(config_group *cg, uint16_t count);
+void destroy_config_group(config_group* cg);
 
-void config_init();
+//void config_init();
 //void config_open(const char* path, const char* mode);
-void config_read(const char* path);
+
 //void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length);
-void config_set_lighthouse(BaseStationData* bsd, uint8_t idx);
+void config_set_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8_t idx);
+
+void config_read(SurviveContext* sctx, const char* path);
+void config_save(SurviveContext* sctx, const char* path);
 
-void config_save(const char* path);
 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);
 
 FLT config_read_float(config_group *cg, const char *tag, const FLT def);
-uint16_t config_read_float_array(config_group *cg, const char *tag, FLT** values, const FLT* def, uint16_t count);
+uint16_t config_read_float_array(config_group *cg, const char *tag, const FLT** values, const FLT* def, uint16_t count);
 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);