diff options
Diffstat (limited to 'controller/fw/src')
| -rw-r--r-- | controller/fw/src/dsss_demod.c | 61 |
1 files changed, 34 insertions, 27 deletions
diff --git a/controller/fw/src/dsss_demod.c b/controller/fw/src/dsss_demod.c index ec54d41..7a2e0cc 100644 --- a/controller/fw/src/dsss_demod.c +++ b/controller/fw/src/dsss_demod.c @@ -11,17 +11,14 @@ #include "simulation.h" #include "generated/dsss_gold_code.h" +#include "generated/dsss_butter_filter.h" /* Generated CWT wavelet LUT */ extern const float * const dsss_cwt_wavelet_table; -struct iir_biquad cwt_filter_bq[3] = { - {.a = {-1.993939440f, 0.993949280f}, .b = {0.2459934300683e-5, 0.4919868601367e-5, 0.2459934300683e-5}}, - {.a = {-1.995557124f, 0.995566972f}, .b = {0.2461930046414e-5, 0.4923860092828e-5, 0.2461930046414e-5}}, - {.a = {-1.998365254f, 0.998375115f}, .b = {0.2465394452097e-5, 0.4930788904195e-5, 0.2465394452097e-5}}, -}; +struct iir_biquad cwt_filter_bq[DSSS_FILTER_CLEN] = {DSSS_FILTER_COEFF}; -float gold_correlate_step(const size_t ncode, const float a[DSSS_CORRELATION_LENGTH], size_t offx); +float gold_correlate_step(const size_t ncode, const float a[DSSS_CORRELATION_LENGTH], size_t offx, bool debug); float cwt_convolve_step(const float v[DSSS_WAVELET_LUT_SIZE], size_t offx); float run_iir(const float x, const int order, const struct iir_biquad q[order], struct iir_biquad_state st[order]); float run_biquad(float x, const struct iir_biquad *const q, struct iir_biquad_state *const restrict st); @@ -34,7 +31,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) { //#define DEBUG_PRINT(...) ((void)0) //#define DEBUG_PRINTN(...) ((void)0) - bool debug = sim_pos % DSSS_CORRELATION_LENGTH == 0; + bool debug = sim_pos % DSSS_CORRELATION_LENGTH == DSSS_CORRELATION_LENGTH-1; if (debug) DEBUG_PRINT("Iteration %zd", sim_pos); //const float peak_group_threshold = 0.05 * DSSS_CORRELATION_LENGTH; //const float hole_patching_threshold = 0.01 * DSSS_CORRELATION_LENGTH; @@ -45,14 +42,14 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) { /* use new, incremented wpos for gold_correlate_step as first element of old data in ring buffer */ for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) - st->correlation[i][st->correlation_wpos] = gold_correlate_step(i, st->signal, st->correlation_wpos); + st->correlation[i][st->correlation_wpos] = gold_correlate_step(i, st->signal, st->signal_wpos, debug); /* debug */ - /* - DEBUG_PRINTN(" correlation: ["); - for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) - DEBUG_PRINTN("%f, ", st->correlation[i][st->correlation_wpos]); - DEBUG_PRINTN("]\n"); - */ + if (debug) { + DEBUG_PRINTN(" correlation: ["); + for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) + DEBUG_PRINTN("%f, ", st->correlation[i][st->correlation_wpos]); + DEBUG_PRINTN("]\n"); + } /* end */ st->correlation_wpos = (st->correlation_wpos + 1) % ARRAY_LENGTH(st->correlation[0]); @@ -60,10 +57,12 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) { for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) cwt[i] = cwt_convolve_step(st->correlation[i], st->correlation_wpos); /* debug */ + /* if (debug) DEBUG_PRINTN(" cwt: ["); for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) if (debug) DEBUG_PRINTN("%f, ", cwt[i]); if (debug) DEBUG_PRINTN("]\n"); + */ /* end */ float avg[DSSS_GOLD_CODE_COUNT]; @@ -82,10 +81,10 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) { int max_ch = st->group.max_ch; int max_idx = st->group.max_idx + 1; bool found = false; - if (debug) DEBUG_PRINTN(" rel: ["); + //if (debug) DEBUG_PRINTN(" rel: ["); for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) { float val = cwt[i] / avg[i]; - if (debug) DEBUG_PRINTN("%f, ", val); + //if (debug) DEBUG_PRINTN("%f, ", val); if (fabs(val) > DSSS_THESHOLD_FACTOR) found = true; @@ -96,7 +95,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) { max_idx = st->group.len; } } - if (debug) DEBUG_PRINTN("]\n"); + //if (debug) DEBUG_PRINTN("]\n"); /* debug */ if (debug) DEBUG_PRINT(" found=%d len=%d idx=%d ch=%d max=%f", @@ -162,19 +161,27 @@ float cwt_convolve_step(const float v[DSSS_WAVELET_LUT_SIZE], size_t offx) { * * [0] https://docs.scipy.org/doc/numpy/reference/generated/numpy.correlate.html */ -float gold_correlate_step(const size_t ncode, const float a[DSSS_CORRELATION_LENGTH], size_t offx) { +float gold_correlate_step(const size_t ncode, const float a[DSSS_CORRELATION_LENGTH], size_t offx, bool debug) { + float acc_outer = 0.0f; uint8_t table_byte = 0; - for (size_t i=0, pos=0; i<DSSS_GOLD_CODE_LENGTH; i++, pos += DSSS_DECIMATION) { - float acc_inner = 0.0f; - for (size_t j=0; j<DSSS_DECIMATION; j++) { - if ((pos&7) == 0) - table_byte = dsss_gold_code_table[ncode][pos>>3]; /* Fetch sequence table item */ - int bv = table_byte & (1<<(pos&7)); /* Extract bit */ - bv = !!bv*2 - 1; /* Map 0, 1 -> -1, 1 */ - acc_inner += a[(offx + i + j) % DSSS_CORRELATION_LENGTH] * bv; /* Multiply item */ + if (debug) DEBUG_PRINTN("Correlate n=%d: ", ncode); + for (size_t i=0; i<DSSS_GOLD_CODE_LENGTH; i++) { + + if ((i&7) == 0) { + table_byte = dsss_gold_code_table[ncode][i>>3]; /* Fetch sequence table item */ + if (debug) DEBUG_PRINTN("|"); } - acc_outer += acc_inner; + int bv = table_byte & (0x80>>(i&7)); /* Extract bit */ + bv = !!bv*2 - 1; /* Map 0, 1 -> -1, 1 */ + if (debug) DEBUG_PRINTN("%s%d\033[0m", bv == 1 ? "\033[92m" : "\033[91m", (bv+1)/2); + + float acc_inner = 0.0f; + for (size_t j=0; j<DSSS_DECIMATION; j++) + acc_inner += a[(offx + i*DSSS_DECIMATION + j) % DSSS_CORRELATION_LENGTH]; /* Multiply item */ + //if (debug) DEBUG_PRINTN("%.2f ", acc_inner); + acc_outer += acc_inner * bv; } + if (debug) DEBUG_PRINTN("\n"); return acc_outer / DSSS_CORRELATION_LENGTH; } |