diff options
author | CNLohr <charles@cnlohr.com> | 2016-12-26 22:24:10 -0500 |
---|---|---|
committer | GitHub <noreply@github.com> | 2016-12-26 22:24:10 -0500 |
commit | a4c008dd28d2dda226bf9f9eace1413519fd76c6 (patch) | |
tree | 8ae47cc208b2d89f6828ad5cf6fb03f0f975d4da /src | |
parent | dfed49687c361ccaae75c7389cddcfd74fbdf82e (diff) | |
parent | 1f141a4cd74c1c9b2d13a204da05ad697fa1c4bf (diff) | |
download | libsurvive-a4c008dd28d2dda226bf9f9eace1413519fd76c6.tar.gz libsurvive-a4c008dd28d2dda226bf9f9eace1413519fd76c6.tar.bz2 |
Merge pull request #13 from jpicht/disambiguate
disambiguator
Diffstat (limited to 'src')
-rw-r--r-- | src/disambiguator.c | 148 | ||||
-rw-r--r-- | src/disambiguator.h | 52 | ||||
-rw-r--r-- | src/survive_data.c | 6 |
3 files changed, 172 insertions, 34 deletions
diff --git a/src/disambiguator.c b/src/disambiguator.c index f641834..f1d310a 100644 --- a/src/disambiguator.c +++ b/src/disambiguator.c @@ -1,23 +1,80 @@ // (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. +// 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, long age ); +inline void disambiguator_discard( struct disambiguator * d ); -void disambiguator_discard( struct disambiguator * d, long age ) +/** + * 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) { @@ -32,41 +89,73 @@ void disambiguator_discard( struct disambiguator * d, long age ) d->max_confidence = confidence; } -inline int disambiguator_find_nearest( struct disambiguator * d, long time, int max_diff ); +/** + * 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, long time, 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 = abs(d->times[i] - time); - if (a < diff) { + 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; + diff = a_2; } } +// if (idx != -1) { +// printf("R %d %d %d\n", idx, d->scores[idx], diff); +// } + return idx; } -pulse_type disambiguator_step( struct disambiguator * d, long time, int length) +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 d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN; + return disambiguator_step_return_helper(d, sweep_possible); } - //printf( "%d %d\n", time, length ); - //printf( "." ); - //time -= length/2; - disambiguator_discard(d, time - 10000000); - int idx = disambiguator_find_nearest(d, time - 400000, 100); - + // 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) { @@ -75,19 +164,34 @@ pulse_type disambiguator_step( struct disambiguator * d, long time, int length) } } - return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN; + return d->state == D_STATE_LOCKED && sweep_possible ? P_SWEEP : P_UNKNOWN; } else { d->scores[idx]++; - if (d->scores[idx] >= 30) { + + // 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; - return d->state == D_STATE_LOCKED ? ( - d->scores[idx] >= d->max_confidence ? P_SYNC : P_SWEEP - ) : P_UNKNOWN; + + // 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 d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN; + return disambiguator_step_return_helper(d, sweep_possible); } diff --git a/src/disambiguator.h b/src/disambiguator.h index 0db19de..8258a18 100644 --- a/src/disambiguator.h +++ b/src/disambiguator.h @@ -4,35 +4,65 @@ #ifndef DISAMBIGUATOR_H #define DISAMBIGUATOR_H -#define DIS_NUM_VALUES 8 +// 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_SYNC = 1, + P_MASTER = 1, P_SWEEP = 2, + P_SLAVE = 3, } pulse_type; +/** + * internal state of the disambiguator + */ struct disambiguator { - long times[DIS_NUM_VALUES]; - int scores[DIS_NUM_VALUES]; + // 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; - long last; + // last sync pulse time + uint32_t last; + // the absolute maximum counter value int max_confidence; + // the last code type seen char code; }; -struct classified_pulse_ { - pulse_type t; - int length; -}; +/** + * 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); -pulse_type disambiguator_step( struct disambiguator * d, long time, int length); -#endif /* DISAMBIGUATOR_H */ +/** + * 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/survive_data.c b/src/survive_data.c index 791bdf6..3e5f14e 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -43,9 +43,13 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * 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 - case P_SYNC: + break; + case P_MASTER: ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, offset ); so->d->code = ((le->length+125)/250) - 12; break; |