diff options
Diffstat (limited to 'controller/fw/src')
-rw-r--r-- | controller/fw/src/dsss_demod.c | 64 | ||||
-rw-r--r-- | controller/fw/src/dsss_demod.h | 33 |
2 files changed, 79 insertions, 18 deletions
diff --git a/controller/fw/src/dsss_demod.c b/controller/fw/src/dsss_demod.c index cc1c34e..1cad6f8 100644 --- a/controller/fw/src/dsss_demod.c +++ b/controller/fw/src/dsss_demod.c @@ -46,17 +46,16 @@ void debug_print_vector(const char *name, size_t len, const float *data, size_t #endif #ifdef SIMULATION -void dsss_demod_step(struct dsss_demod_state *st, float new_value, size_t sim_pos, int record_channel) { +void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts, int record_channel) { bool debug = (record_channel == -1) - && (sim_pos > 1000) - && (sim_pos % DSSS_CORRELATION_LENGTH == DSSS_CORRELATION_LENGTH-1); + && (ts > 1000) + && (ts % DSSS_CORRELATION_LENGTH == DSSS_CORRELATION_LENGTH-1); - if (debug) DEBUG_PRINT("Iteration %zd: signal=%f", sim_pos, new_value); + if (debug) DEBUG_PRINT("Iteration %zd: signal=%f", ts, new_value); #else void dsss_demod_step(struct dsss_demod_state *st, float new_value) { #endif - //const float peak_group_threshold = 0.05 * DSSS_CORRELATION_LENGTH; //const float hole_patching_threshold = 0.01 * DSSS_CORRELATION_LENGTH; st->signal[st->signal_wpos] = new_value; @@ -89,7 +88,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) { float max_val = st->group.max; int max_ch = st->group.max_ch; - int max_idx = st->group.max_idx + 1; + int max_ts = st->group.max_ts; bool found = false; for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) { float val = cwt[i] / avg[i]; @@ -100,7 +99,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) { if (fabs(val) > fabs(max_val)) { max_val = val; max_ch = i; - max_idx = st->group.len; + max_ts = ts; } } @@ -109,7 +108,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) { st->group.len++; st->group.max = max_val; st->group.max_ch = max_ch; - st->group.max_idx = max_idx; + st->group.max_ts = max_ts; return; } @@ -120,15 +119,60 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) { /* A group ended. Process result. */ if (record_channel == -1) DEBUG_PRINT("GROUP FOUND: %8d len=%3d max=%f ch=%d offx=%d", - sim_pos, st->group.len, st->group.max, st->group.max_ch, st->group.max_idx); + ts, st->group.len, st->group.max, st->group.max_ch, st->group.max_ts); /* reset grouping state */ st->group.len = 0; - st->group.max_idx = 0; + st->group.max_ts = 0; st->group.max_ch = 0; st->group.max = 0.0f; } +float score_group(const struct group *g, uint64_t ts) { + return fabs(g->max); /* Possibly at time penalty 1/(ts-max_ts) later */ +} + +ssize_t group_cache_insertion_index(const struct group *g, const struct group *cache, size_t cache_size, uint64_t ts) { + float min_score = INFINITY; + ssize_t min_idx = -1; + for (size_t i=0; i<cache_size; i++) { + /* If we find an empty or expired entry, use that */ + if (cache[i].max_ts == 0 || ts - cache[i].max_ts > group_cache_expiration) + return i; + + /* Otherwise check weakest entry */ + float score = score_group(&cache[i]); + if (score < min_score) { + min_idx = i; + min_score = score; + } + } + + /* Return weakest group if weaker than candidate */ + if (min_score < score_group(g)) + return min_idx; + + return -1; +} + +void group_received(struct dsss_demod_state *st, uint64_t ts) { + /* TODO make these constants configurable from Makefile */ + const uint64_t group_cache_expiration = DSSS_CORRELATION_LENGTH * DSSS_GROUP_CACHE_SIZE; + + /* Insert into group cache if space is available or there is a weaker entry to replace */ + ssize_t found = group_cache_insertion_index(&st->group, st->group_cache, DSSS_GROUP_CACHE_SIZE); + if (!found) + return; /* Nothing changed */ + st->group_cache[found] = st->group; + + float mean_phase = 0.0; + for (size_t i=0; i<DSSS_GROUP_CACHE_SIZE; i++) + mean_phase += (st->group_cache[i].max_ts) % DSSS_CORRELATION_LENGTH; + mean_phase /= DSSS_GROUP_CACHE_SIZE; + + +} + float run_iir(const float x, const int order, const struct iir_biquad q[order], struct iir_biquad_state st[order]) { float intermediate = x; for (int i=0; i<(order+1)/2; i++) diff --git a/controller/fw/src/dsss_demod.h b/controller/fw/src/dsss_demod.h index b6947bc..30ec3f7 100644 --- a/controller/fw/src/dsss_demod.h +++ b/controller/fw/src/dsss_demod.h @@ -5,6 +5,8 @@ #define DSSS_GOLD_CODE_COUNT ((1<<DSSS_GOLD_CODE_NBITS) + 1) #define DSSS_CORRELATION_LENGTH (DSSS_GOLD_CODE_LENGTH * DSSS_DECIMATION) +#define PAYLOAD_DATA_BYTE ((PAYLOAD_DATA_BIT+7)/8) + struct iir_biquad { float a[2]; float b[3]; @@ -18,6 +20,24 @@ struct cwt_iir_filter_state { struct iir_biquad_state st[3]; }; +struct { + int len; /* length of group in samples */ + float max; /* signed value of largest peak in group on any channel */ + uint64_t max_ts; /* absolute position of above peak */ + int max_ch; /* channel (gold sequence index) of above peak */ +} group; + +struct decoder_state { + int last_phase; + int candidate_phase; + + float last_score; + float candidate_score; + + uint8_t data[PAYLOAD_DATA_BYTE]; + int data_pos; +}; + struct dsss_demod_state { float signal[DSSS_CORRELATION_LENGTH]; size_t signal_wpos; @@ -27,18 +47,15 @@ struct dsss_demod_state { struct cwt_iir_filter_state cwt_filter[DSSS_GOLD_CODE_COUNT]; - struct { - int len; /* length of group in samples */ - float max; /* signed value of largest peak in group on any channel */ - int max_idx; /* position of above peak counted from start of group */ - int max_ch; /* channel (gold sequence index) of above peak */ - } group; + struct group group; + + struct group group_cache[DSSS_GROUP_CACHE_SIZE]; }; #ifdef SIMULATION -void dsss_demod_step(struct dsss_demod_state *st, float new_value, size_t sim_pos, int record_channel); +void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts, int record_channel); #else /* SIMULATION */ -void dsss_demod_step(struct dsss_demod_state *st, float new_value); +void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts); #endif /* SIMULATION */ #endif /* __DSSS_DEMOD_H__ */ |