aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
authorcnlohr <lohr85@gmail.com>2017-02-14 01:00:33 -0500
committercnlohr <lohr85@gmail.com>2017-02-14 01:00:33 -0500
commit35e08aaa24bd01e6ace453f89ddb73a6bb0508b0 (patch)
treebb101b68eeabd8a85cd4e062ecca4b7dab140ecd /src
parentf782146df94b3b54965c2aed696d49e86870046d (diff)
downloadlibsurvive-35e08aaa24bd01e6ace453f89ddb73a6bb0508b0.tar.gz
libsurvive-35e08aaa24bd01e6ace453f89ddb73a6bb0508b0.tar.bz2
Still not working, but getting closer to dynamic OOTX decoding.
Diffstat (limited to 'src')
-rw-r--r--src/ootx_decoder.c163
-rw-r--r--src/ootx_decoder.h41
-rw-r--r--src/survive_cal.c68
-rw-r--r--src/survive_cal.h44
-rw-r--r--src/survive_data.c12
-rw-r--r--src/survive_internal.h26
-rw-r--r--src/survive_process.c15
7 files changed, 359 insertions, 10 deletions
diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c
new file mode 100644
index 0000000..b8746bc
--- /dev/null
+++ b/src/ootx_decoder.c
@@ -0,0 +1,163 @@
+// (C) 2016 Joshua Allen, MIT/x11 License.
+//
+//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
+
+/* ootx data decoder */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+#include "ootx_decoder.h"
+#include <crc32.h>
+
+//char* fmt_str = "L Y HMD %d 5 1 206230 %d\n";
+
+
+void (*ootx_packet_clbk)(ootx_decoder_context *ctx, ootx_packet* packet) = NULL;
+
+void ootx_init_decoder_context(ootx_decoder_context *ctx) {
+ ctx->buf_offset = 0;
+ ctx->bits_written = 0;
+
+ ctx->preamble = 0XFFFFFFFF;
+ ctx->bits_processed = 0;
+ ctx->found_preamble = 0;
+
+ memset( ctx->buffer, 0, sizeof( ctx->buffer ) );
+ ctx->payload_size = (uint16_t*)ctx->buffer;
+ *(ctx->payload_size) = 0;
+}
+/*
+void ootx_init_buffer() {
+ buffer = (uint8_t*)malloc(MAX_BUFF_SIZE);
+ payload_size = (uint16_t*)buffer;
+ *payload_size = 0;
+}
+*/
+
+/*
+ how to decode pulses
+ ticks>2000 && delta>100000== master lighthouse
+ ticks>2000 && delta>10000 == slave lighthouse
+*/
+
+int8_t ootx_decode_lighthouse_number(uint8_t last_num, uint32_t ticks, int32_t delta) {
+ if (ticks<2000) return -1; //sweep
+ if ((ticks > 2000) & (delta>100000)) return 0; //master
+ if ((ticks > 2000) & (delta>10000)) return last_num+1; //a slave
+ return -1;
+}
+
+/*
+uint8_t ootx_decode_bit(uint32_t ticks) {
+ ticks = ((ticks/500)*500)+500;
+
+ ticks-=3000;
+ if (ticks>=2000) { ticks-=2000; }
+ if (ticks>=1000) { return 0xFF; }
+
+ return 0x00;
+}*/
+
+uint8_t ootx_detect_preamble(ootx_decoder_context *ctx, uint8_t dbit) {
+ ctx->preamble <<= 1;
+ ctx->preamble |= (0x01 & dbit);
+ if ((ctx->preamble & 0x0001ffff) == 0x01) return 1;
+ return 0;
+}
+
+void ootx_reset_buffer(ootx_decoder_context *ctx) {
+ ctx->buf_offset = 0;
+ ctx->buffer[ctx->buf_offset] = 0;
+ ctx->bits_written = 0;
+ *(ctx->payload_size) = 0;
+}
+
+void ootx_inc_buffer_offset(ootx_decoder_context *ctx) {
+ ++(ctx->buf_offset);
+
+// assert(ctx->buf_offset<MAX_BUFF_SIZE);
+
+ /* the buffer is going to overflow, wrap the buffer and don't write more data until the preamble is found again */
+ if(ctx->buf_offset>=MAX_OOTX_BUFF_SIZE) {
+ ctx->buf_offset = 0;
+ ctx->found_preamble = 0;
+ }
+
+ ctx->buffer[ctx->buf_offset] = 0;
+}
+
+void ootx_write_to_buffer(ootx_decoder_context *ctx, uint8_t dbit) {
+ uint8_t *current_byte = ctx->buffer + ctx->buf_offset;
+// printf("%d\n", dbit);
+ *current_byte >>= 1;
+ *current_byte |= (0x80 & dbit);
+ ++(ctx->bits_written);
+ if (ctx->bits_written>7) {
+ ctx->bits_written=0;
+// printf("%d\n", *current_byte);
+ ootx_inc_buffer_offset(ctx);
+ }
+}
+
+void ootx_process_bit(ootx_decoder_context *ctx, uint8_t dbit) {
+ //uint8_t dbit = ootx_decode_bit(length);
+ ++(ctx->bits_processed);
+
+// printf("z %d %d\n", bits_processed,dbit);
+// printf("d %d\n", bits_processed,dbit);
+
+ if ( ootx_detect_preamble(ctx, dbit) ) {
+ /* data stream can start over at any time so we must
+ always look for preamble bits */
+ printf("Preamble found\n");
+ ootx_reset_buffer(ctx);
+ ctx->bits_processed = 0;
+ ctx->found_preamble = 1;
+ }
+ else if(ctx->bits_processed>16) {
+ //every 17th bit needs to be dropped (sync bit)
+// printf("drop %d\n", dbit);
+ if( !dbit )
+ {
+ printf( "Sync bit missing\n" );
+ ootx_reset_buffer(ctx);
+ }
+ ctx->bits_processed = 0;
+ }
+ else if (ctx->found_preamble > 0)
+ {
+ /* only write to buffer if the preamble is found.
+ if the buffer overflows, found_preamble will be cleared
+ and writing will stop. data would be corrupted, so there is no point in continuing
+ */
+
+ ootx_write_to_buffer(ctx, dbit);
+printf( "%d / %d -> ", ctx->buf_offset, *ctx->payload_size );
+int k;
+for( k = 0; k < 32; k++ )
+{
+ printf( "%02x ", ctx->buffer[k] );
+}
+printf( "\n" );
+ if (ctx->buf_offset >= (*(ctx->payload_size)+6)) {
+ /* once we have a complete ootx packet, send it out in the callback */
+ ootx_packet op;
+
+ op.length = *(ctx->payload_size);
+ op.data = ctx->buffer+2;
+ op.crc32 = *(uint32_t*)(ctx->buffer+2+op.length);
+
+ uint32_t crc = crc32(0xffffffff,op.data,op.length);
+
+ if (crc != op.crc32) {
+ printf("CRC mismatch\n");
+ }
+
+ if ((crc == op.crc32) && ootx_packet_clbk) ootx_packet_clbk(ctx, &op);
+
+ ootx_reset_buffer(ctx);
+ }
+ }
+}
diff --git a/src/ootx_decoder.h b/src/ootx_decoder.h
new file mode 100644
index 0000000..5df5ba2
--- /dev/null
+++ b/src/ootx_decoder.h
@@ -0,0 +1,41 @@
+// (C) 2016 Joshua Allen, MIT/x11 License.
+//
+//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
+
+#ifndef OOTX_DECODER_H
+#define OOTX_DECODER_H
+
+#include <stddef.h>
+#include <stdint.h>
+
+#define MAX_OOTX_BUFF_SIZE 1024
+
+typedef struct {
+ uint16_t length;
+ uint8_t* data;
+ uint32_t crc32;
+} ootx_packet;
+
+typedef struct {
+ uint8_t buffer[MAX_OOTX_BUFF_SIZE];
+ uint16_t buf_offset;
+ uint8_t bits_written;
+ uint16_t* payload_size;
+
+ uint32_t preamble;
+ uint8_t bits_processed;
+ uint8_t found_preamble;
+ void * user;
+ int user1;
+} ootx_decoder_context;
+
+
+//void ootx_init_buffer();
+void ootx_process_bit(ootx_decoder_context *ctx, uint8_t dbit); //dbit MUST be 0x00 or 0xFF
+void ootx_init_decoder_context(ootx_decoder_context *ctx);
+int8_t ootx_decode_lighthouse_number(uint8_t last_num, uint32_t ticks, int32_t delta);
+
+
+extern void (*ootx_packet_clbk)(ootx_decoder_context * ctx, ootx_packet* packet);
+
+#endif
diff --git a/src/survive_cal.c b/src/survive_cal.c
new file mode 100644
index 0000000..4223e70
--- /dev/null
+++ b/src/survive_cal.c
@@ -0,0 +1,68 @@
+// (C) 2016, 2017 Joshua Allen, MIT/x11 License.
+// (C) 2016, 2017 <>< C. N. Lohr, Under MIT/x11 License.
+
+// All OOTX code was written by J. Allen. Rest of the code is probably mostly CNLohr.
+
+#include "survive_cal.h"
+#include "survive_internal.h"
+
+void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
+{
+ struct SurviveContext * ctx = (struct SurviveContext*)(ct->user);
+ struct SurviveCalData * cd = ctx->calptr;
+ int id = ct->user1;
+
+ printf( "Got OOTX packet %d %p\n", id, cd );
+}
+
+
+void survive_cal_install( struct SurviveContext * ctx )
+{
+ int i;
+ struct SurviveCalData * cd = ctx->calptr = calloc( 1, sizeof( struct SurviveCalData ) );
+
+ for( i = 0; i < NUM_LIGHTHOUSES; i++ )
+ {
+ ootx_init_decoder_context(&cd->ootx_decoders[i]);
+ cd->ootx_decoders[i].user = ctx;
+ cd->ootx_decoders[i].user1 = i;
+ }
+
+ cd->stage = 1;
+
+ ootx_packet_clbk = ootx_packet_clbk_d;
+
+ ctx->calptr = cd;
+}
+
+
+void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length )
+{
+ struct SurviveContext * ctx = so->ctx;
+ struct SurviveCalData * cd = ctx->calptr;
+
+ if( !cd ) return;
+
+ switch( cd->stage )
+ {
+ default:
+ case 0: //Default, inactive.
+ break;
+
+ case 1:
+ //Collecting OOTX data.
+ if( sensor_id < 0 )
+ {
+ int lhid = -sensor_id-1;
+ if( lhid >= NUM_LIGHTHOUSES-1 && so->codename[0] == 'H' )
+ {
+ uint8_t dbit = (acode & 2)?0xff:0x00;
+ printf( "%s %d %d %d\n", so->codename, lhid, acode, dbit );
+ ootx_process_bit( &cd->ootx_decoders[lhid], dbit );
+ }
+ }
+
+ break;
+ }
+}
+
diff --git a/src/survive_cal.h b/src/survive_cal.h
new file mode 100644
index 0000000..a74bf95
--- /dev/null
+++ b/src/survive_cal.h
@@ -0,0 +1,44 @@
+// (C) 2016, 2017 <>< C. N. Lohr, Under MIT/x11 License.
+
+// All OOTX code was written by J. Allen. Rest of the code is probably mostly CNLohr.
+
+#ifndef _SURVIVE_CAL_H
+#define _SURVIVE_CAL_H
+
+//This is a file that is intended for use with capturing vive data during the
+//setup phase. This and survive_cal.c/.h should not be included on embedded
+//uses of libsurvive.
+
+//This file handles the following:
+// 1: Decoding the OOTX data from the lighthouses.
+// 2: Setting OOTX props in the survive context.
+// 3: Collect a bunch of data with the vive pointed up and the watchment to either side.
+// 4: Running the code to find the lighthouses.
+// 5: Setting the information needed to develop the worldspace model in the SurviveContext.
+
+
+#include <stdint.h>
+#include "ootx_decoder.h"
+#include "survive_internal.h"
+
+void survive_cal_install( struct SurviveContext * ctx );
+int survive_cal_get_status( struct SurviveContext * ctx, char * description, int max_data );
+
+//void survive_cal_teardown( struct SurviveContext * ctx );
+
+//Called from survive_default_light_process
+void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
+
+struct SurviveCalData
+{
+ //Stage:
+ // 0: Idle
+ // 1: Collecting OOTX data.
+ int stage;
+
+ //OOTX Data is sync'd off of
+ ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES];
+};
+
+#endif
+
diff --git a/src/survive_data.c b/src/survive_data.c
index b79f96e..c557951 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -81,7 +81,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
else
{
ssn = ++so->sync_set_number;
- if( so->sync_set_number > 1 )
+ if( so->sync_set_number >= NUM_LIGHTHOUSES )
{
SV_INFO( "Warning. Received an extra, unassociated sync pulse." );
ssn = so->sync_set_number = -1;
@@ -106,12 +106,20 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
}
}
}
+
+
+
//See if this is a valid actual pulse.
else if( le->length < 1800 && le->length > 40 && delta > 30000 && ssn >= 0 )
{
int32_t dl = so->last_time[0];
int32_t tpco = so->last_length[0];
+
+#if NUM_LIGHTHOUSES != 2
+ #error You are going to have to fix the code around here to allow for something other than two base stations.
+#endif
+
//Adding length
//Long pulse-code from IR flood.
//Make sure it fits nicely into a divisible-by-500 time.
@@ -128,6 +136,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
acode_array[0] = (acode_array[0]>>1) - 6;
acode_array[1] = (acode_array[1]>>1) - 6;
+
int acode = acode_array[0];
if( !so->did_handle_ootx )
@@ -135,7 +144,6 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
int32_t delta1 = so->last_time[0] - so->recent_sync_time;
int32_t delta2 = so->last_time[1] - so->last_time[0];
- //XXX Axlecrusher -> Add your code here!!!
ctx->lightproc( so, -1, acode_array[0], delta1, so->last_time[0], so->last_length[0] );
ctx->lightproc( so, -2, acode_array[1], delta2, so->last_time[1], so->last_length[1] );
diff --git a/src/survive_internal.h b/src/survive_internal.h
index 446f3c0..11c9d89 100644
--- a/src/survive_internal.h
+++ b/src/survive_internal.h
@@ -62,6 +62,22 @@ struct SurviveUSBInterface
//This is defined in survive.h
struct SurviveObject;
+struct SurviveCalData;
+
+struct BaseStationData
+{
+ uint8_t PositionSet:1;
+ float Position[3];
+ float Quaternion[4];
+
+ uint8_t OOTXSet:1;
+ uint32_t BaseStationID;
+ float fcalphase[2];
+ float fcaltilt[2];
+ float fcalcurve[2];
+ float fcalgibpha[2];
+ float fcalgibmag[2];
+};
struct SurviveContext
{
@@ -75,9 +91,15 @@ struct SurviveContext
light_process_func lightproc;
imu_process_func imuproc;
- //Data Subsystem
+ //Calibration data:
+ struct BaseStationData bsd[NUM_LIGHTHOUSES];
+
+ struct SurviveCalData * calptr; //If and only if the calibration subsystem is attached.
+
+ //Data Subsystem. These should be last, as there may be additional surviveobjects.
struct SurviveObject headset;
- struct SurviveObject watchman[2];
+ struct SurviveObject watchman[2]; //Currently only two supported watchmen.
+
};
diff --git a/src/survive_process.c b/src/survive_process.c
index d3a8c4a..184532b 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -1,15 +1,19 @@
//<>< (C) 2016 C. N. Lohr, FULLY Under MIT/x11 License.
//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
-#include "survive_internal.h"
+#include "survive_cal.h"
-
-int bufferpts[32*2];
-char buffermts[32*128];
-int buffertimeto[32];
+//XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to
+//proper angles, then from there perform the rest of the solution.
void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length )
{
+
+ if( so->ctx->calptr )
+ {
+ survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length );
+ }
+
//TODO: Writeme!
}
@@ -18,4 +22,3 @@ void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro
//TODO: Writeme!
}
-