summaryrefslogtreecommitdiff
path: root/controller/fw/src
diff options
context:
space:
mode:
Diffstat (limited to 'controller/fw/src')
-rw-r--r--controller/fw/src/dsss_demod.c61
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;
}