aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/disambiguator.c197
-rw-r--r--src/disambiguator.h68
-rw-r--r--src/ootx_decoder.c287
-rw-r--r--src/ootx_decoder.h72
-rw-r--r--src/survive.c47
-rw-r--r--src/survive_cal.c195
-rw-r--r--src/survive_cal.h57
-rw-r--r--src/survive_data.c185
-rw-r--r--src/survive_internal.h35
-rw-r--r--src/survive_process.c51
-rw-r--r--src/survive_usb.c6
11 files changed, 846 insertions, 354 deletions
diff --git a/src/disambiguator.c b/src/disambiguator.c
deleted file mode 100644
index f1d310a..0000000
--- a/src/disambiguator.c
+++ /dev/null
@@ -1,197 +0,0 @@
-// (C) 2016 Julian Picht, MIT/x11 License.
-//
-// All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
-
-//
-// The theory behind this disambiguator is, that if we just track all pulses and if one could be a sync pulse, we look back in time,
-// if we saw as sync pulse X samples ago than it is probably a sync pulse.
-//
-// X can be 20000 or 400000, depending if it came from the master or the slave.
-//
-
-#include "disambiguator.h"
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <stdbool.h>
-
-typedef uint8_t pulse_data;
-
-/**
- * Translate pulse length to pulse SKIP, DATA, AXIS
- * @param length Length of the pulse in (1/48000000) seconds
- * @return pulse data
- */
-pulse_data get_pulse_data(uint32_t length) {
- uint16_t temp = length - 2880;
-
-#if BETTER_SAFE_THAN_FAST
- if (temp < 0 || length > 6525) {
- return -1;
- }
-#endif
-
- if ((temp % 500) < 150) {
- return temp / 500;
- }
-
- return -1;
-}
-
-const uint32_t pulse_types[] = {
- 0, 1, 0, 1,
- 2, 3, 2, 3,
-};
-
-#define PULSE_BIT_AXIS 0x1
-#define PULSE_BIT_DATA 0x2
-#define PULSE_BIT_SKIP 0x4
-
-#define PULSE_DATA(D) ((D >> 1)&0x1)
-#define PULSE_AXIS(D) (D&0x01)
-#define PULSE_SKIP(D) ((D >> 2)&0x1)
-
-void disambiguator_init( struct disambiguator * d ) {
- memset(&(d->times), 0x0, sizeof(d->times));
- memset(&(d->scores), 0x0, sizeof(d->scores));
-
- d->state = D_STATE_UNLOCKED;
- d->last = 0;
- d->max_confidence = 0;
-}
-
-inline void disambiguator_discard( struct disambiguator * d );
-
-/**
- * Drop all data that is outdated
- * @param d
- * @param age Maximum age of data we care to keep
- */
-void disambiguator_discard( struct disambiguator * d )
-{
- long age;
- if (d->state == D_STATE_LOCKED) {
- age = d->last - 400000;
- } else {
- age = 1000000;
- }
- int confidence = 0;
- for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) {
- if (d->times[i] != 0 && d->times[i] < age) {
- d->times[i] = 0;
- d->scores[i] = 0;
- } else {
- if (d->scores[i] > confidence) {
- confidence = d->scores[i];
- }
- }
- }
- d->max_confidence = confidence;
-}
-
-/**
- * Find the index that has the best likelyhood too match up with the timestamp given
- * @param t1 Rising edge time, where we expect to find the last sync pulse, if this is a master pulse
- * @param t2 Rising edge time, where we expect to find the last sync pulse, if this is a slave pulse
- * @param max_diff Maximum difference we are prepared to accept
- * @return index inside d->times, if we found something, -1 otherwise
- */
-inline int disambiguator_find_nearest( struct disambiguator * d, uint32_t t1, uint32_t t2, int max_diff );
-
-int disambiguator_find_nearest( struct disambiguator * d, uint32_t t1, uint32_t t2, int max_diff )
-{
- int diff = max_diff; // max allowed diff for a match
- int idx = -1;
- for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) {
- if (d->times[i] == 0) continue;
-
- int a_1 = abs(d->times[i] - t1);
- int a_2 = abs(d->times[i] - t2);
-
-// printf("T %d %d %d\n", time, i, a);
- if (a_1 < diff) {
- idx = i;
- diff = a_1;
- } else if (a_2 < diff) {
- idx = i;
- diff = a_2;
- }
- }
-
-// if (idx != -1) {
-// printf("R %d %d %d\n", idx, d->scores[idx], diff);
-// }
-
- return idx;
-}
-
-pulse_type disambiguator_step_return_helper( struct disambiguator * d, bool sweep_possible ) {
- if (d->state == D_STATE_LOCKED && sweep_possible) {
- return P_SWEEP;
- }
- return P_UNKNOWN;
-}
-
-pulse_type disambiguator_step( struct disambiguator * d, uint32_t time, int length)
-{
- uint32_t diff = time - d->last;
- bool sweep_possible = (diff > 70000 && diff < 350000);
-
- // all smaller pulses are most probably sweeps
- // TODO: check we are inside the time window of actual sweeps
- if (length < 2750) {
- return disambiguator_step_return_helper(d, sweep_possible);
- }
-
- // we expected to see a sync pulse earlier ...
- if (time - d->last > 401000) {
- d->state = D_STATE_UNLOCKED;
- }
-
- // discard all data, that is so old, we don't care about it anymore
- disambiguator_discard(d);
-
- // find the best match for our timestamp and presumed offset
- int idx = disambiguator_find_nearest(d, time - 400000, time - 20000, 1000);
-
- // We did not find a matching pulse, so try find a place to record the current
- // one's time of arrival.
- if (idx == -1) {
- for (int i = 0; i < DIS_NUM_VALUES; ++i) {
- if (d->times[i] == 0) {
- d->times[i] = time;
- break;
- }
- }
-
- return d->state == D_STATE_LOCKED && sweep_possible ? P_SWEEP : P_UNKNOWN;
- } else {
- d->scores[idx]++;
-
- // we need to be reasonably sure, that we have the right pulses
- if (d->scores[idx] >= DIS_NUM_PULSES_BEFORE_LOCK) {
- d->state = D_STATE_LOCKED;
- }
-
- // if the offset is about 20000 ticks, then this is a slave pulse
- if (diff < 21000) {
- if (d->state == D_STATE_LOCKED) {
- return P_SLAVE;
- }
-
- return P_UNKNOWN;
- }
-
- d->times[idx] = time;
- d->last = time;
-
- // TODO: why do we need to check the confidence level here?
- if (d->state == D_STATE_LOCKED && d->scores[idx] >= d->max_confidence) {
- return P_MASTER;
- }
-
- return P_UNKNOWN;
- }
-
- return disambiguator_step_return_helper(d, sweep_possible);
-}
diff --git a/src/disambiguator.h b/src/disambiguator.h
deleted file mode 100644
index 8258a18..0000000
--- a/src/disambiguator.h
+++ /dev/null
@@ -1,68 +0,0 @@
-// (C) 2016 Julian Picht, MIT/x11 License.
-//
-//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
-#ifndef DISAMBIGUATOR_H
-#define DISAMBIGUATOR_H
-
-// Determines the number of samples stored in the disambiguator struct.
-// Has to be higher than the maximum number of pulses expected between sync pulses.
-#define DIS_NUM_VALUES 48
-#define DIS_NUM_PULSES_BEFORE_LOCK 30
-#include <stdint.h>
-
-/**
- * internal disambiguator state
- */
-typedef enum {
- D_STATE_INVALID = 0,
- D_STATE_LOCKED = 1,
- D_STATE_UNLOCKED = -1,
-} dis_state;
-
-/**
- * classification result
- */
-typedef enum {
- P_UNKNOWN = 0,
- P_MASTER = 1,
- P_SWEEP = 2,
- P_SLAVE = 3,
-} pulse_type;
-
-/**
- * internal state of the disambiguator
- */
-struct disambiguator {
- // the timestamps of the recorded pulses
- uint32_t times[DIS_NUM_VALUES];
- // countes how many sync pulses we have seen, that match the time offset at the same offset
- uint16_t scores[DIS_NUM_VALUES];
- // current state
- dis_state state;
- // last sync pulse time
- uint32_t last;
- // the absolute maximum counter value
- int max_confidence;
- // the last code type seen
- char code;
-};
-
-
-/**
- * Initialize a new disambiguator. calloc or memset with 0x00 will work just as well.
- *
- * @param d Pointer to the struct
- */
-void disambiguator_init( struct disambiguator * d);
-
-/**
- * Feed in one pulse to have if classified.
- *
- * @param d Pointer to disambiguator state
- * @param time Rising edge of the pulse
- * @param length Length of the pulse
- * @return Classification result
- */
-pulse_type disambiguator_step( struct disambiguator * d, uint32_t time, int length);
-
-#endif /* DISAMBIGUATOR_H */ \ No newline at end of file
diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c
new file mode 100644
index 0000000..8ec16f2
--- /dev/null
+++ b/src/ootx_decoder.c
@@ -0,0 +1,287 @@
+// (C) 2017 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 <zlib.h>
+#include <assert.h>
+#include "ootx_decoder.h"
+//#include "crc32.h"
+
+//char* fmt_str = "L Y HMD %d 5 1 206230 %d\n";
+
+#define MAX_BUFF_SIZE 64
+
+void (*ootx_packet_clbk)(ootx_decoder_context * ctx, ootx_packet* packet) = NULL;
+void (*ootx_bad_crc_clbk)(ootx_decoder_context * ctx, ootx_packet* packet, uint32_t crc) = NULL;
+
+void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit);
+
+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;
+
+ ctx->buffer = (uint8_t*)malloc(MAX_BUFF_SIZE);
+ ctx->payload_size = (uint16_t*)ctx->buffer;
+ *(ctx->payload_size) = 0;
+}
+
+void ootx_free_decoder_context(ootx_decoder_context *ctx) {
+ free(ctx->buffer);
+ ctx->buffer = NULL;
+ ctx->payload_size = NULL;
+}
+
+uint8_t ootx_decode_bit(uint32_t length) {
+ uint8_t t = (length - 2750) / 500; //why 2750?
+// return ((t & 0x02)>0)?0xFF:0x00; //easier if we need to bitshift right
+ return ((t & 0x02)>>1);
+}
+
+uint8_t ootx_detect_preamble(ootx_decoder_context *ctx, uint8_t dbit) {
+ ctx->preamble <<= 1;
+// ctx->preamble |= (0x01 & dbit);
+ ctx->preamble |= dbit;
+ if ((ctx->preamble & 0x0003ffff) == 0x00000001) return 1;
+ return 0;
+}
+
+void ootx_reset_buffer(ootx_decoder_context *ctx) {
+ ctx->buf_offset = 0;
+ ctx->buffer[0] = 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_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;
+
+ *current_byte <<= 1;
+// *current_byte |= (0x01 & dbit);
+ *current_byte |= dbit;
+
+ ++(ctx->bits_written);
+ if (ctx->bits_written>7) {
+ ctx->bits_written=0;
+// printf("%d\n", *current_byte);
+ ootx_inc_buffer_offset(ctx);
+ }
+}
+
+uint8_t ootx_process_bit(ootx_decoder_context *ctx, uint32_t length) {
+ uint8_t dbit = ootx_decode_bit(length);
+ ootx_pump_bit( ctx, dbit );
+ return dbit;
+}
+
+void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) {
+// uint8_t dbit = ootx_decode_bit(length);
+ ++(ctx->bits_processed);
+
+ 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("Bad sync bit\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);
+
+ uint16_t padded_length = *(ctx->payload_size);
+ padded_length += (padded_length&0x01); //extra null byte if odd
+
+/* int k;
+ printf( ":" );
+ for( k = 0; k < 36; k++ )
+ {
+ printf( "%02x ", ctx->buffer[k] );
+ }
+ printf( "\n" );*/
+
+ if (ctx->buf_offset >= (padded_length+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*)(op.data+padded_length);
+
+ uint32_t crc = crc32( 0L, Z_NULL, 0 );
+ crc = crc32( crc, op.data,op.length);
+
+ if (crc != op.crc32) {
+ if (ootx_bad_crc_clbk != NULL) ootx_bad_crc_clbk(ctx, &op,crc);
+ }
+ else if (ootx_packet_clbk != NULL) {
+ ootx_packet_clbk(ctx,&op);
+ }
+
+ ootx_reset_buffer(ctx);
+ }
+ }
+}
+
+uint8_t* get_ptr(uint8_t* data, uint8_t bytes, uint16_t* idx) {
+ uint8_t* x = data + *idx;
+ *idx += bytes;
+ return x;
+}
+
+/* simply doing:
+float f = 0;
+uint32_t *ftmp = (uint32_t*)&f; //use the allocated floating point memory
+This can cause problem when strict aliasing (-O2) is used.
+Reads and writes to f and ftmp would be considered independent and could be
+be reordered by the compiler. A union solves that problem.
+*/
+union iFloat {
+ uint32_t i;
+ float f;
+};
+
+float _half_to_float(uint8_t* data) {
+ uint16_t x = *(uint16_t*)data;
+ union iFloat fnum;
+ fnum.f = 0;
+
+ //sign
+ fnum.i = (x & 0x8000)<<16;
+
+ if ((x & 0x7FFF) == 0) return fnum.f; //signed zero
+
+ if ((x & 0x7c00) == 0) {
+ //denormalized
+ x = (x&0x3ff)<<1; //only mantissa, advance intrinsic bit forward
+ uint8_t e = 0;
+ //shift until intrinsic bit of mantissa overflows into exponent
+ //increment exponent each time
+ while ((x&0x0400) == 0) {
+ x<<=1;
+ e++;
+ }
+ fnum.i |= ((uint32_t)(112-e))<<23; //bias exponent to 127, half floats are biased 15 so only need to go 112 more.
+ fnum.i |= ((uint32_t)(x&0x3ff))<<13; //insert mantissa
+ return fnum.f;
+ }
+
+ if((x&0x7c00) == 0x7c00) {
+ //for infinity, fraction is 0
+ //for NaN, fraction is anything non zero
+ //we could just copy in bits and not shift, but the mantissa of a NaN can have meaning
+ fnum.i |= 0x7f800000 | ((uint32_t)(x & 0x3ff))<<13;
+ return fnum.f;
+ }
+
+ fnum.i |= ((((uint32_t)(x & 0x7fff)) + 0x1c000u) << 13);
+
+ return fnum.f;
+}
+
+void init_lighthouse_info_v6(lighthouse_info_v6* lhi, uint8_t* data) {
+ uint16_t idx = 0;
+ /*
+ uint16_t fw_version;//Firmware version (bit 15..6), protocol version (bit 5..0)
+ uint32_t id; //Unique identifier of the base station
+ float fcal_0_phase; //"phase" for rotor 0
+ float fcal_1_phase; //"phase" for rotor 1
+ float fcal_0_tilt; //"tilt" for rotor 0
+ float fcal_1_tilt; //"tilt" for rotor 1
+ uint8_t sys_unlock_count; //Lowest 8 bits of the rotor desynchronization counter
+ uint8_t hw_version; //Hardware version
+ float fcal_0_curve; //"curve" for rotor 0
+ float fcal_1_curve; //"curve" for rotor 1
+ int8_t accel_dir_x; //"orientation vector"
+ int8_t accel_dir_y; //"orientation vector"
+ int8_t accel_dir_z; //"orientation vector"
+ float fcal_0_gibphase; //"gibbous phase" for rotor 0 (normalized angle)
+ float fcal_1_gibphase; //"gibbous phase" for rotor 1 (normalized angle)
+ float fcal_0_gibmag; //"gibbous magnitude" for rotor 0
+ float fcal_1_gibmag; //"gibbous magnitude" for rotor 1
+ uint8_t mode_current; //Currently selected mode (default: 0=A, 1=B, 2=C)
+ uint8_t sys_faults; //"fault detect flags" (should be 0)
+ */
+
+ lhi->fw_version = *(uint16_t*)get_ptr(data,sizeof(uint16_t),&idx);
+ lhi->id = *(uint32_t*)get_ptr(data,sizeof(uint32_t),&idx);
+ lhi->fcal_0_phase = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+ lhi->fcal_1_phase = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+ lhi->fcal_0_tilt = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+ lhi->fcal_1_tilt = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+ lhi->sys_unlock_count = *get_ptr(data,sizeof(uint8_t),&idx);
+ lhi->hw_version = *get_ptr(data,sizeof(uint8_t),&idx);
+ lhi->fcal_0_curve = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+ lhi->fcal_1_curve = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+ lhi->accel_dir_x = *(int8_t*)get_ptr(data,sizeof(uint8_t),&idx);
+ lhi->accel_dir_y = *(int8_t*)get_ptr(data,sizeof(uint8_t),&idx);
+ lhi->accel_dir_z = *(int8_t*)get_ptr(data,sizeof(uint8_t),&idx);
+ lhi->fcal_0_gibphase = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+ lhi->fcal_1_gibphase = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+ lhi->fcal_0_gibmag = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+ lhi->fcal_1_gibmag = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+ lhi->mode_current = *get_ptr(data,sizeof(uint8_t),&idx);
+ lhi->sys_faults = *get_ptr(data,sizeof(uint8_t),&idx);
+
+}
+
+void print_lighthouse_info_v6(lighthouse_info_v6* lhi) {
+
+ printf("\t%X\n\t%X\n\t%f\n\t%f\n\t%f\n\t%f\n\t%d\n\t%d\n\t%f\n\t%f\n\t%d\n\t%d\n\t%d\n\t%f\n\t%f\n\t%f\n\t%f\n\t%d\n\t%d\n",
+ lhi->fw_version,
+ lhi->id,
+ lhi->fcal_0_phase,
+ lhi->fcal_1_phase,
+ lhi->fcal_0_tilt,
+ lhi->fcal_1_tilt,
+ lhi->sys_unlock_count,
+ lhi->hw_version,
+ lhi->fcal_0_curve,
+ lhi->fcal_1_curve,
+ lhi->accel_dir_x,
+ lhi->accel_dir_y,
+ lhi->accel_dir_z,
+ lhi->fcal_0_gibphase,
+ lhi->fcal_1_gibphase,
+ lhi->fcal_0_gibmag,
+ lhi->fcal_1_gibmag,
+ lhi->mode_current,
+ lhi->sys_faults);
+}
diff --git a/src/ootx_decoder.h b/src/ootx_decoder.h
new file mode 100644
index 0000000..8ddf527
--- /dev/null
+++ b/src/ootx_decoder.h
@@ -0,0 +1,72 @@
+// (C) 2017 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>
+
+typedef struct {
+ uint16_t length;
+ uint8_t* data;
+ uint32_t crc32;
+} ootx_packet;
+
+typedef struct {
+ uint8_t* buffer;
+ uint16_t buf_offset;
+ uint8_t bits_written;
+ uint16_t* payload_size;
+
+ uint32_t preamble;
+ uint8_t bits_processed;
+ uint8_t found_preamble;
+
+ uint8_t bit_count[2];
+
+ void * user;
+ int user1;
+} ootx_decoder_context;
+
+
+typedef float float16;
+
+typedef struct {
+ uint16_t fw_version;//Firmware version (bit 15..6), protocol version (bit 5..0)
+ uint32_t id; //Unique identifier of the base station
+ float16 fcal_0_phase; //"phase" for rotor 0
+ float16 fcal_1_phase; //"phase" for rotor 1
+ float16 fcal_0_tilt; //"tilt" for rotor 0
+ float16 fcal_1_tilt; //"tilt" for rotor 1
+ uint8_t sys_unlock_count; //Lowest 8 bits of the rotor desynchronization counter
+ uint8_t hw_version; //Hardware version
+ float16 fcal_0_curve; //"curve" for rotor 0
+ float16 fcal_1_curve; //"curve" for rotor 1
+ int8_t accel_dir_x; //"orientation vector"
+ int8_t accel_dir_y; //"orientation vector"
+ int8_t accel_dir_z; //"orientation vector"
+ float16 fcal_0_gibphase; //"gibbous phase" for rotor 0 (normalized angle)
+ float16 fcal_1_gibphase; //"gibbous phase" for rotor 1 (normalized angle)
+ float16 fcal_0_gibmag; //"gibbous magnitude" for rotor 0
+ float16 fcal_1_gibmag; //"gibbous magnitude" for rotor 1
+ uint8_t mode_current; //Currently selected mode (default: 0=A, 1=B, 2=C)
+ uint8_t sys_faults; //"fault detect flags" (should be 0)
+} lighthouse_info_v6;
+
+void init_lighthouse_info_v6(lighthouse_info_v6* lhi, uint8_t* data);
+void print_lighthouse_info_v6(lighthouse_info_v6* lhi);
+
+void ootx_init_decoder_context(ootx_decoder_context *ctx);
+void ootx_free_decoder_context(ootx_decoder_context *ctx);
+
+uint8_t ootx_process_bit(ootx_decoder_context *ctx, uint32_t length);
+void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit);
+
+uint8_t ootx_decode_bit(uint32_t length);
+
+extern void (*ootx_packet_clbk)(ootx_decoder_context *ctx, ootx_packet* packet);
+extern void (*ootx_bad_crc_clbk)(ootx_decoder_context *ctx, ootx_packet* packet, uint32_t crc);
+
+#endif
diff --git a/src/survive.c b/src/survive.c
index af4d804..aab2ae6 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -8,7 +8,7 @@
#include <jsmn.h>
#include <string.h>
#include <zlib.h>
-#include "disambiguator.h"
+
static int jsoneq(const char *json, jsmntok_t *tok, const char *s) {
if (tok->type == JSMN_STRING && (int) strlen(s) == tok->end - tok->start &&
@@ -30,7 +30,7 @@ static void survivenote( struct SurviveContext * ctx, const char * fault )
fprintf( stderr, "Info: %s\n", fault );
}
-static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, char * ct0conf, SV_FLOAT ** floats_out, jsmntok_t * t, int i )
+static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i )
{
int k;
int pts = t[i+1].size;
@@ -43,7 +43,7 @@ static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so,
{
tk = &t[i+2+k*4];
- float vals[3];
+ FLT vals[3];
int m;
for( m = 0; m < 3; m++ )
{
@@ -60,7 +60,7 @@ static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so,
memcpy( ctt, ct0conf + tk->start, elemlen );
ctt[elemlen] = 0;
- float f = atof( ctt );
+ FLT f = atof( ctt );
int id = so->nr_locations*3+m;
(*floats_out)[id] = f;
}
@@ -144,24 +144,14 @@ struct SurviveContext * survive_init()
ctx->lightproc = survive_default_light_process;
ctx->imuproc = survive_default_imu_process;
+ ctx->angleproc = survive_default_angle_process;
ctx->headset.ctx = ctx;
memcpy( ctx->headset.codename, "HMD", 4 );
-#ifndef USE_OLD_DISAMBIGUATOR
- ctx->headset.d = calloc( 1, sizeof( struct disambiguator ) );
-#endif
-
ctx->watchman[0].ctx = ctx;
memcpy( ctx->watchman[0].codename, "WM0", 4 );
-#ifndef USE_OLD_DISAMBIGUATOR
- ctx->watchman[0].d = calloc( 1, sizeof( struct disambiguator ) );
-#endif
-
ctx->watchman[1].ctx = ctx;
memcpy( ctx->watchman[1].codename, "WM1", 4 );
-#ifndef USE_OLD_DISAMBIGUATOR
- ctx->watchman[1].d = calloc( 1, sizeof( struct disambiguator ) );
-#endif
//USB must happen last.
if( r = survive_usb_init( ctx ) )
@@ -175,6 +165,18 @@ struct SurviveContext * survive_init()
if( LoadConfig( ctx, &ctx->watchman[0], 2, 0, 1 ) ) { SV_INFO( "Watchman 0 config issue." ); }
if( LoadConfig( ctx, &ctx->watchman[1], 3, 0, 1 ) ) { SV_INFO( "Watchman 1 config issue." ); }
+ ctx->headset.timebase_hz = ctx->watchman[0].timebase_hz = ctx->watchman[1].timebase_hz = 48000000;
+ ctx->headset.pulsedist_max_ticks = ctx->watchman[0].pulsedist_max_ticks = ctx->watchman[1].pulsedist_max_ticks = 500000;
+ ctx->headset.pulselength_min_sync = ctx->watchman[0].pulselength_min_sync = ctx->watchman[1].pulselength_min_sync = 2200;
+ ctx->headset.pulse_in_clear_time = ctx->watchman[0].pulse_in_clear_time = ctx->watchman[1].pulse_in_clear_time = 35000;
+ ctx->headset.pulse_max_for_sweep = ctx->watchman[0].pulse_max_for_sweep = ctx->watchman[1].pulse_max_for_sweep = 1800;
+
+ ctx->headset.pulse_synctime_offset = ctx->watchman[0].pulse_synctime_offset = ctx->watchman[1].pulse_synctime_offset = 20000;
+ ctx->headset.pulse_synctime_slack = ctx->watchman[0].pulse_synctime_slack = ctx->watchman[1].pulse_synctime_slack = 5000;
+
+ ctx->headset.timecenter_ticks = ctx->headset.timebase_hz / 240;
+ ctx->watchman[0].timecenter_ticks = ctx->watchman[0].timebase_hz / 240;
+ ctx->watchman[1].timecenter_ticks = ctx->watchman[1].timebase_hz / 240;
/*
int i;
int locs = ctx->headset.nr_locations;
@@ -206,7 +208,7 @@ fail_gracefully:
return 0;
}
-void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_fnptr fbp )
+void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_func fbp )
{
if( fbp )
ctx->notefunction = fbp;
@@ -214,7 +216,7 @@ void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_fnptr
ctx->notefunction = survivenote;
}
-void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_fnptr fbp )
+void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_func fbp )
{
if( fbp )
ctx->faultfunction = fbp;
@@ -238,6 +240,17 @@ void survive_install_imu_fn( struct SurviveContext * ctx, imu_process_func fbp
ctx->imuproc = survive_default_imu_process;
}
+
+void survive_install_angle_fn( struct SurviveContext * ctx, angle_process_func fbp )
+{
+ if( fbp )
+ ctx->angleproc = fbp;
+ else
+ ctx->angleproc = survive_default_angle_process;
+}
+
+
+
void survive_close( struct SurviveContext * ctx )
{
survive_usb_close( ctx );
diff --git a/src/survive_cal.c b/src/survive_cal.c
new file mode 100644
index 0000000..7b0a824
--- /dev/null
+++ b/src/survive_cal.c
@@ -0,0 +1,195 @@
+// (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"
+#include <math.h>
+#include <string.h>
+
+#define PTS_BEFORE_COMMON 32
+#define NEEDED_COMMON_POINTS 20
+
+static void handle_calibration( struct SurviveCalData *cd );
+static void reset_calibration( struct SurviveCalData * cd );
+
+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;
+
+ SV_INFO( "Got OOTX packet %d %p\n", id, cd );
+
+ lighthouse_info_v6 v6;
+ init_lighthouse_info_v6(&v6, packet->data);
+
+ struct BaseStationData * b = &ctx->bsd[id];
+ //print_lighthouse_info_v6(&v6);
+
+ b->BaseStationID = v6.id;
+ b->fcalphase[0] = v6.fcal_0_phase;
+ b->fcalphase[1] = v6.fcal_1_phase;
+ b->fcaltilt[0] = tan(v6.fcal_0_tilt);
+ b->fcaltilt[1] = tan(v6.fcal_1_tilt); //XXX??? Is this right? See https://github.com/cnlohr/libsurvive/issues/18
+ b->fcalcurve[0] = v6.fcal_0_curve;
+ b->fcalcurve[1] = v6.fcal_1_curve;
+ b->fcalgibpha[0] = v6.fcal_0_gibphase;
+ b->fcalgibpha[1] = v6.fcal_1_gibphase;
+ b->fcalgibmag[0] = v6.fcal_0_gibmag;
+ b->fcalgibmag[1] = v6.fcal_1_gibmag;
+ b->OOTXSet = 1;
+}
+
+int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length )
+{
+ struct SurviveCalData * cd = ctx->calptr;
+
+ switch( cd->stage )
+ {
+ case 0:
+ return snprintf( description, description_length, "Not calibrating" );
+ case 1:
+ return snprintf( description, description_length, "Collecting OOTX Data (%d:%d)", cd->ootx_decoders[0].buf_offset, cd->ootx_decoders[1].buf_offset );
+ case 2:
+ if( cd->found_common )
+ {
+ return snprintf( description, description_length, "Collecting Sweep Data %d/%d", cd->peak_counts, DRPTS );
+ }
+ else
+ {
+ return snprintf( description, description_length, "Searching for common watchman cal %d/%d", cd->peak_counts, PTS_BEFORE_COMMON );
+ }
+ default:
+ return snprintf( description, description_length, "Unkown calibration state" );
+ }
+}
+
+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 && so->codename[0] == 'H' )
+ {
+ uint8_t dbit = (acode & 2)>>1;
+ ootx_pump_bit( &cd->ootx_decoders[lhid], dbit );
+ }
+ int i;
+ for( i = 0; i < NUM_LIGHTHOUSES; i++ )
+ if( ctx->bsd[i].OOTXSet == 0 ) break;
+ if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on.
+ }
+ break;
+ case 2: //Taking in angle data.
+ break;
+ }
+}
+
+void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+{
+ struct SurviveContext * ctx = so->ctx;
+ struct SurviveCalData * cd = ctx->calptr;
+
+ if( !cd ) return;
+
+ switch( cd->stage )
+ {
+ default:
+ case 1: //Collecting OOTX data. (Don't do anything here, yet.)
+ case 0: //Default, inactive.
+ break;
+ case 2:
+ {
+ int sensid = sensor_id;
+ if( strcmp( so->codename, "WM0" ) == 0 )
+ sensid += 32;
+ if( strcmp( so->codename, "WM1" ) == 1 )
+ sensid += 64;
+
+ int lighthouse = acode>>2;
+ int axis = acode & 1;
+ int ct = cd->all_counts[sensid][lighthouse][axis]++;
+ cd->all_lengths[sensid][lighthouse][axis][ct] = length;
+ cd->all_angles[sensid][lighthouse][axis][ct] = angle;
+ if( ct > cd->peak_counts )
+ {
+ cd->peak_counts = ct;
+ if( ct >= DRPTS )
+ handle_calibration( cd ); //This will also reset all cals.
+ }
+
+ //TODO: Determine if there is a sensor on a watchman visible from both lighthouses.
+ if( sensid >= 32 && !cd->found_common )
+ {
+ int k;
+ int ok = 1;
+ for( k = 0; k < NUM_LIGHTHOUSES; k++ )
+ {
+
+ if( cd->all_counts[sensid][k][0] < NEEDED_COMMON_POINTS || cd->all_counts[sensid][k][1] < NEEDED_COMMON_POINTS )
+ {
+ ok = 0;
+ break;
+ }
+ }
+ if( ok ) cd->found_common = 1;
+ }
+
+ if( cd->peak_counts > PTS_BEFORE_COMMON && !cd->found_common )
+ {
+ reset_calibration( cd );
+ }
+
+ break;
+ }
+ }
+}
+
+static void reset_calibration( struct SurviveCalData * cd )
+{
+ memset( cd->all_counts, 0, sizeof( cd->all_counts ) );
+ cd->peak_counts = 0;
+ cd->found_common = 0;
+}
+
+static void handle_calibration( struct SurviveCalData *cd )
+{
+ //Do stuff.
+
+ reset_calibration( cd );
+}
diff --git a/src/survive_cal.h b/src/survive_cal.h
new file mode 100644
index 0000000..42ff1ee
--- /dev/null
+++ b/src/survive_cal.h
@@ -0,0 +1,57 @@
+// (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 description_length );
+
+//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 );
+void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
+
+#define MAX_TO_CAL 96
+#define DRPTS 512
+
+struct SurviveCalData
+{
+ //OOTX Data is sync'd off of the sync pulses coming from the lighthouses.
+ ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES];
+
+ //For statistics-gathering phase.
+ FLT all_lengths[MAX_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS];
+ FLT all_angles[MAX_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS];
+ int16_t all_counts[MAX_TO_CAL][NUM_LIGHTHOUSES][2];
+ int16_t peak_counts;
+ int8_t found_common;
+
+ //Stage:
+ // 0: Idle
+ // 1: Collecting OOTX data.
+ int8_t stage;
+};
+
+
+
+#endif
+
diff --git a/src/survive_data.c b/src/survive_data.c
index d380d4a..ad834cf 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -11,7 +11,6 @@
//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
#include "survive_internal.h"
-#include "disambiguator.h"
#include <stdint.h>
#include <string.h>
@@ -32,89 +31,164 @@ struct LightcapElement
//This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le )
{
- struct SurviveContext * ct = so->ctx;
- int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
+ struct SurviveContext * ctx = so->ctx;
+ //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
// printf( "%s %d %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp, le->timestamp-so->tsl );
so->tsl = le->timestamp;
- if( le->length < 20 ) return;
-#ifndef USE_OLD_DISAMBIGUATOR
- int32_t offset = le->timestamp - so->d->last;
- switch( disambiguator_step( so->d, le->timestamp, le->length ) ) {
- default:
- case P_SLAVE:
- // this is only interesting for the OOTX data
- break;
- case P_UNKNOWN:
- // not currently locked
- break;
- case P_MASTER:
- ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, offset );
- so->d->code = ((le->length+125)/250) - 12;
- break;
- case P_SWEEP:
- if (so->d->code & 1) return;
- ct->lightproc( so, le->sensor_id, so->d->code >> 1, offset, le->timestamp, le->length );
- break;
+ if( le->length < 20 ) return; ///Assuming 20 is an okay value for here.
+
+ //The sync pulse finder is taking Charles's old disambiguator code and mixing it with a more linear
+ //version of Julian Picht's disambiguator, available in 488c5e9. Removed afterwards into this
+ //unified driver.
+ int ssn = so->sync_set_number;
+ if( ssn < 0 ) ssn = 0;
+ int last_sync_time = so->last_time [ssn];
+ int last_sync_length = so->last_length[ssn];
+ int32_t delta = le->timestamp - last_sync_time; //Handle time wrapping (be sure to be int32)
+
+ if( delta < -so->pulsedist_max_ticks || delta > so->pulsedist_max_ticks )
+ {
+ //Reset pulse, etc.
+ so->sync_set_number = -1;
+ delta = so->pulsedist_max_ticks;
}
-#else
- if( le->length > 2200 ) //Pulse longer indicates a sync pulse.
+
+
+ if( le->length > so->pulselength_min_sync ) //Pulse longer indicates a sync pulse.
{
- int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
- if( deltat > 2000 || deltat < -2000 ) //New pulse. (may be inverted)
+ int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length;
+
+ so->did_handle_ootx = 0;
+
+ if( is_new_pulse )
{
- //See if this is a unique pulse, or another one in the same set we need to look at.
- if( le->timestamp - so->last_master_time > 1500 + so->last_photo_length )
+ int is_master_sync_pulse = delta > so->pulse_in_clear_time /*40000*/;
+
+ if( is_master_sync_pulse )
+ {
+ ssn = so->sync_set_number = 0;
+ so->last_time[ssn] = le->timestamp;
+ so->last_length[ssn] = le->length;
+ }
+ else if( so->sync_set_number == -1 )
+ {
+ //Do nothing.
+ }
+ else
{
- // check if it is a slave pulse
- if (le->timestamp - so->last_master_time < 70000) {
- so->last_slave_time = le->timestamp;
- return;
+ ssn = ++so->sync_set_number;
+ if( so->sync_set_number >= NUM_LIGHTHOUSES )
+ {
+ SV_INFO( "Warning. Received an extra, unassociated sync pulse." );
+ ssn = so->sync_set_number = -1;
+ }
+ else
+ {
+ so->last_time[ssn] = le->timestamp;
+ so->last_length[ssn] = le->length;
}
-
-// printf("%10u %10u %6u %6d\n", so->last_master_time, le->timestamp, (le->length - 2750)/500, (int32_t)le->timestamp - (int32_t)so->last_master_time);
- so->last_master_time = le->timestamp;
- so->last_photo_length = le->length;
- ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat );
- deltat = 0;
}
}
-
- //Find longest pulse-length from device in our window and use that one.
- if( le->length > so->last_photo_length )
+ else
{
-// printf("%10u %10u %6d %6d\n", so->last_master_time, le->timestamp, (le->length - 2750)/500, (int32_t)le->timestamp - (int32_t)so->last_master_time);
- so->last_master_time = le->timestamp;
- so->last_photo_length = le->length;
+ //Find the longest pulse.
+ if( le->length > last_sync_length )
+ {
+ if( so->last_time[ssn] > le->timestamp )
+ {
+ so->last_time[ssn] = le->timestamp;
+ so->last_length[ssn] = le->length;
+ }
+ }
}
}
+
+
+
//See if this is a valid actual pulse.
- else if( le->length < 1800 && le->length > 40 && ( le->timestamp - so->last_master_time < 380000 ) )
+ else if( le->length < so->pulse_max_for_sweep && delta > so->pulse_in_clear_time && ssn >= 0 )
{
- int32_t dl = so->last_master_time;
- int32_t tpco = so->last_photo_length;
+ 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.
- int32_t acode = (tpco+125+50)/250; //+10, seems ike that's
- if( acode & 1 ) return;
- acode >>= 1;
- acode -= 6;
- if (acode > 3) {
- dl = so->last_slave_time;
+ int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
+
+ int32_t acode_array[2] =
+ {
+ (so->last_length[0]+main_divisor+50)/(main_divisor*2), //+50 adds a small offset and seems to help always get it right.
+ (so->last_length[1]+main_divisor+50)/(main_divisor*2), //Check the +50 in the future to see how well this works on a variety of hardware.
+ };
+
+ //XXX: TODO: Capture error count here.
+ if( acode_array[0] & 1 ) return;
+ if( acode_array[1] & 1 ) return;
+
+ 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 )
+ {
+ int32_t delta1 = so->last_time[0] - so->recent_sync_time;
+ int32_t delta2 = so->last_time[1] - so->last_time[0];
+
+ 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] );
+
+ so->recent_sync_time = so->last_time[1];
+
+ //Throw out everything if our sync pulses look like they're bad.
+
+ int32_t center_1 = so->timecenter_ticks*2 - so->pulse_synctime_offset;
+ int32_t center_2 = so->pulse_synctime_offset;
+ int32_t slack = so->pulse_synctime_slack;
+
+ if( delta1 < center_1 - slack || delta1 > center_1 + slack )
+ {
+ //XXX: TODO: Count faults.
+ so->sync_set_number = -1;
+ return;
+ }
+
+ if( delta2 < center_2 - slack || delta2 > center_2 + slack )
+ {
+ //XXX: TODO: Count faults.
+ so->sync_set_number = -1;
+ return;
+ }
+
+ so->did_handle_ootx = 1;
}
- //printf( "%s / %d %d ++ %d %d\n", so->codename, dl, tpco, offset_from, acode );
+
+ if (acode > 3) {
+ if( ssn == 0 )
+ {
+ SV_INFO( "Warning: got a slave marker but only got a master sync." );
+ }
+ dl = so->last_time[1];
+ tpco = so->last_length[1];
+ }
int32_t offset_from = le->timestamp - dl + le->length/2;
//Make sure pulse is in valid window
if( offset_from < 380000 && offset_from > 70000 )
{
- ct->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length );
+ ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length );
}
}
else
@@ -122,7 +196,6 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
//printf( "FAIL %d %d - %d = %d\n", le->length, so->last_photo_time, le->timestamp, so->last_photo_time - le->timestamp );
//Runt pulse, or no sync pulses available.
}
-#endif
}
diff --git a/src/survive_internal.h b/src/survive_internal.h
index 446f3c0..0d0b8a4 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
{
@@ -70,14 +86,22 @@ struct SurviveContext
struct libusb_device_handle * udev[MAX_USB_DEVS];
struct SurviveUSBInterface uiface[MAX_INTERFACES];
- text_feedback_fnptr faultfunction;
- text_feedback_fnptr notefunction;
+ text_feedback_func faultfunction;
+ text_feedback_func notefunction;
light_process_func lightproc;
imu_process_func imuproc;
+ angle_process_func angleproc;
+
- //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.
+
};
@@ -90,9 +114,6 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno,
//Accept Data from backend.
void survive_data_cb( struct SurviveUSBInterface * si );
-//Accept higher-level data.
-void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
-void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id );
#endif
diff --git a/src/survive_process.c b/src/survive_process.c
index d3a8c4a..75453da 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -1,21 +1,58 @@
//<>< (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 )
{
- //TODO: Writeme!
+ struct SurviveContext * ctx = so->ctx;
+ int base_station = acode >> 2;
+ int axis = acode & 1;
+
+ if( ctx->calptr )
+ {
+ survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length );
+ }
+
+ if( base_station > NUM_LIGHTHOUSES ) return;
+
+ //No loner need sync information past this point.
+ if( sensor_id < 0 ) return;
+ FLT angle = (timeinsweep - so->timecenter_ticks) * (1./so->timecenter_ticks * 3.14159265359/2.0);
+
+ //Need to now do angle correction.
+#if 1
+ struct BaseStationData * bsd = &ctx->bsd[base_station];
+
+ //XXX TODO: This seriously needs to be worked on. See: https://github.com/cnlohr/libsurvive/issues/18
+ angle += bsd->fcalphase[axis];
+// angle += bsd->fcaltilt[axis] * predicted_angle(axis1);
+
+ //TODO!!!
+#endif
+
+ FLT length_sec = length / (FLT)so->timebase_hz;
+ ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle );
}
+
+void survive_default_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+{
+ struct SurviveContext * ctx = so->ctx;
+ if( ctx->calptr )
+ {
+ survive_cal_angle( so, sensor_id, acode, timecode, length, angle );
+ }
+
+ //TODO: Writeme!
+}
+
+
void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
{
//TODO: Writeme!
}
-
diff --git a/src/survive_usb.c b/src/survive_usb.c
index cf12280..aec76db 100644
--- a/src/survive_usb.c
+++ b/src/survive_usb.c
@@ -125,7 +125,8 @@ static inline int getupdate_feature_report(libusb_device_handle* dev, uint16_t i
static inline int hid_get_feature_report_timeout(libusb_device_handle* device, uint16_t interface, unsigned char *buf, size_t len )
{
int ret;
- for (unsigned i = 0; i < 100; i++)
+ uint8_t i = 0;
+ for (i = 0; i < 100; i++)
{
ret = getupdate_feature_report(device, interface, buf, len);
if( ret != -9 && ( ret != -1 || errno != EPIPE ) ) return ret;
@@ -146,6 +147,7 @@ int survive_usb_init( struct SurviveContext * ctx )
}
int i;
+ int16_t j;
libusb_device** devs;
int ret = libusb_get_device_list(ctx->usbctx, &devs);
@@ -201,7 +203,7 @@ int survive_usb_init( struct SurviveContext * ctx )
}
libusb_set_auto_detach_kernel_driver( ctx->udev[i], 1 );
- for (int j = 0; j < conf->bNumInterfaces; j++ )
+ for (j = 0; j < conf->bNumInterfaces; j++ )
{
#if 0
if (libusb_kernel_driver_active(ctx->udev[i], j) == 1) {