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.c182
-rw-r--r--controller/fw/src/dsss_demod.h23
2 files changed, 162 insertions, 43 deletions
diff --git a/controller/fw/src/dsss_demod.c b/controller/fw/src/dsss_demod.c
index 1cad6f8..32d6104 100644
--- a/controller/fw/src/dsss_demod.c
+++ b/controller/fw/src/dsss_demod.c
@@ -2,6 +2,8 @@
#include <unistd.h>
#include <stdbool.h>
#include <math.h>
+#include <stdlib.h>
+#include <assert.h>
#include <arm_math.h>
@@ -18,11 +20,15 @@ extern const float * const dsss_cwt_wavelet_table;
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, 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);
void debug_print_vector(const char *name, size_t len, const float *data, size_t stride, bool index, bool debug);
+static float gold_correlate_step(const size_t ncode, const float a[DSSS_CORRELATION_LENGTH], size_t offx, bool debug);
+static float cwt_convolve_step(const float v[DSSS_WAVELET_LUT_SIZE], size_t offx);
+static float run_iir(const float x, const int order, const struct iir_biquad q[order], struct iir_biquad_state st[order]);
+static float run_biquad(float x, const struct iir_biquad *const q, struct iir_biquad_state *const restrict st);
+static void matcher_init(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE]);
+static void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE],
+ uint64_t ts, int peak_ch, float peak_ampl);
+static void group_received(struct dsss_demod_state *st, uint64_t ts);
#ifdef SIMULATION
void debug_print_vector(const char *name, size_t len, const float *data, size_t stride, bool index, bool debug) {
@@ -45,11 +51,19 @@ void debug_print_vector(const char *name, size_t len, const float *data, size_t
void debug_print_vector(const char *name, size_t len, const float *data, size_t stride, bool index, bool debug) {}
#endif
+void dsss_demod_init(struct dsss_demod_state *st) {
+ memset(st, 0, sizeof(*st));
+ matcher_init(st->matcher_cache);
+}
+
#ifdef SIMULATION
void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts, int record_channel) {
+ bool debug = false;
+ /*
bool debug = (record_channel == -1)
&& (ts > 1000)
&& (ts % DSSS_CORRELATION_LENGTH == DSSS_CORRELATION_LENGTH-1);
+ */
if (debug) DEBUG_PRINT("Iteration %zd: signal=%f", ts, new_value);
#else
@@ -93,16 +107,19 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) {
float val = cwt[i] / avg[i];
- if (fabs(val) > DSSS_THESHOLD_FACTOR)
- found = true;
-
if (fabs(val) > fabs(max_val)) {
max_val = val;
max_ch = i;
max_ts = ts;
+
+ if (fabs(val) > DSSS_THESHOLD_FACTOR)
+ found = true;
}
}
+ /* FIXME: skipped sample handling here */
+ matcher_tick(st->matcher_cache, ts, max_ch, max_val);
+
if (found) {
/* Continue ongoing group */
st->group.len++;
@@ -120,6 +137,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
if (record_channel == -1)
DEBUG_PRINT("GROUP FOUND: %8d len=%3d max=%f ch=%d offx=%d",
ts, st->group.len, st->group.max, st->group.max_ch, st->group.max_ts);
+ group_received(st, ts);
/* reset grouping state */
st->group.len = 0;
@@ -128,49 +146,139 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
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 */
+/* Map a sequence match to a data symbol. This maps the sequence's index number to the 2nd to n+2nd bit of the result,
+ * and maps the polarity of detection to the LSb. 5-bit example:
+ *
+ * [0, S, S, S, S, S, S, P] ; S ^= symbol index (0 - 2^n+1), P ^= symbol polarity
+ *
+ * Symbol polarity is preserved from transmitter to receiver. The symbol index is n+1 bit instead of n bit since we have
+ * 2^n+1 symbols to express, one too many for an n-bit index.
+ */
+uint8_t decode_peak(int peak_ch, float peak_ampl) {
+ return (peak_ch<<1) | (peak_ampl > 0);
}
-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;
+void matcher_init(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE]) {
+ for (size_t i=0; i<DSSS_MATCHER_CACHE_SIZE; i++)
+ states[i].last_phase = -1; /* mark as inactive */
+}
- /* Otherwise check weakest entry */
- float score = score_group(&cache[i]);
- if (score < min_score) {
- min_idx = i;
- min_score = score;
+/* TODO make these constants configurable from Makefile */
+const int group_phase_tolerance = (int)(DSSS_CORRELATION_LENGTH * 0.10);
+
+void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE], uint64_t ts, int peak_ch, float peak_ampl) {
+ /* TODO make these constants configurable from Makefile */
+ const float skip_sampling_depreciation = 0.2f; /* 0.0 -> no depreciation, 1.0 -> complete disregard */
+ const float score_depreciation = 0.1f; /* 0.0 -> no depreciation, 1.0 -> complete disregard */
+ const uint64_t current_phase = ts % DSSS_CORRELATION_LENGTH;
+
+ for (size_t i=0; i<DSSS_MATCHER_CACHE_SIZE; i++) {
+ if (states[i].last_phase == -1)
+ continue; /* Inactive entry */
+
+ if (current_phase == states[i].last_phase) {
+ /* Skip sampling */
+ float score = fabs(peak_ampl) * (1.0f - skip_sampling_depreciation);
+ if (score > states[i].candidate_score) {
+ /* We win, update candidate */
+ states[i].candidate_score = score;
+ states[i].candidate_phase = current_phase;
+ states[i].candidate_data = decode_peak(peak_ch, peak_ampl);
+ }
+ }
+
+ /* Note of caution on group_phase_tolerance: Group detection has some latency since a group is only considered
+ * "detected" after signal levels have fallen back below the detection threshold. This means we only get to
+ * process a group a couple ticks after its peak. We have to make sure the window is still open at this point.
+ * This means we have to match against group_phase_tolerance should a little bit loosely.
+ */
+ if (abs(states[i].last_phase - current_phase) == group_phase_tolerance + DSSS_DECIMATION) {
+ /* Process window results */
+ states[i].data[ states[i].data_pos ] = states[i].candidate_data;
+ states[i].data_pos = states[i].data_pos + 1;
+ states[i].last_score = score_depreciation * states[i].last_score +
+ (1.0f - score_depreciation) * states[i].candidate_score;
+ states[i].candidate_score = 0.0f;
+
+ if (states[i].data_pos == TRANSMISSION_SYMBOLS) {
+ /* Frame received completely */
+ DEBUG_PRINT("match on index %d phase %d score %.5f", i, states[i].last_phase, states[i].last_score);
+ handle_dsss_received(states[i].data);
+ states[i].last_phase = -1; /* invalidate entry */
+ }
}
}
+}
+
+static float gaussian(float a, float b, float c, float x) {
+ float n = x-b;
+ return a*expf(-n*n / (2.0f* c*c));
+}
- /* Return weakest group if weaker than candidate */
- if (min_score < score_group(g))
- return min_idx;
- return -1;
+static float score_group(const struct group *g, int phase_delta) {
+ /* TODO make these constants configurable from Makefile */
+ const float distance_func_phase_tolerance = 10.0f;
+ return fabsf(g->max) * gaussian(1.0f, 0.0f, distance_func_phase_tolerance, phase_delta);
}
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;
+ static_assert(group_phase_tolerance > 10); /* FIXME debug, remove */
- /* 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;
+ const int group_phase = st->group.max_ts % DSSS_CORRELATION_LENGTH;
+ /* This is the score of a decoding starting at this group (with no context) */
+ float base_score = score_group(&st->group, 0);
- 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 min_score = INFINITY;
+ ssize_t min_idx = -1;
+ ssize_t empty_idx = -1;
+ for (size_t i=0; i<DSSS_MATCHER_CACHE_SIZE; i++) {
+ /* Search for entries with matching phase */
+
+ /* This is the score of this group given the cached decoding at [i] */
+ int phase_delta = st->matcher_cache[i].last_phase - group_phase;
+ if (abs(phase_delta) <= group_phase_tolerance) {
+
+ float group_score = score_group(&st->group, phase_delta);
+ if (st->matcher_cache[i].candidate_score < group_score) {
+ st->matcher_cache[i].candidate_score = group_score;
+ st->matcher_cache[i].candidate_phase = group_phase;
+ st->matcher_cache[i].candidate_data = decode_peak(st->group.max_ch, st->group.max);
+ }
+ }
-
+ /* Search for empty entries */
+ if (st->matcher_cache[i].last_phase == -1)
+ empty_idx = i;
+
+ /* Search for weakest entry */
+ float score = st->matcher_cache[i].last_score;
+ if (score < min_score) {
+ min_idx = i;
+ min_score = score;
+ }
+ }
+
+ /* If we found empty entries, replace one by a new decoding starting at this group */
+ if (empty_idx >= 0) {
+ st->matcher_cache[empty_idx].last_phase = group_phase;
+ st->matcher_cache[empty_idx].candidate_score = base_score;
+ st->matcher_cache[empty_idx].last_score = base_score;
+ st->matcher_cache[empty_idx].candidate_phase = group_phase;
+ st->matcher_cache[empty_idx].candidate_data = decode_peak(st->group.max_ch, st->group.max);
+ st->matcher_cache[empty_idx].data_pos = 0;
+ }
+
+ /* If the weakest decoding in cache is weaker than a new decoding starting here, replace it */
+ if (min_score < base_score) {
+ assert(min_idx >= 0);
+ st->matcher_cache[min_idx].last_phase = group_phase;
+ st->matcher_cache[min_idx].candidate_score = base_score;
+ st->matcher_cache[min_idx].last_score = base_score;
+ st->matcher_cache[min_idx].candidate_phase = group_phase;
+ st->matcher_cache[min_idx].candidate_data = decode_peak(st->group.max_ch, st->group.max);
+ st->matcher_cache[min_idx].data_pos = 0;
+ }
}
float run_iir(const float x, const int order, const struct iir_biquad q[order], struct iir_biquad_state st[order]) {
diff --git a/controller/fw/src/dsss_demod.h b/controller/fw/src/dsss_demod.h
index 30ec3f7..b9e75d2 100644
--- a/controller/fw/src/dsss_demod.h
+++ b/controller/fw/src/dsss_demod.h
@@ -5,6 +5,9 @@
#define DSSS_GOLD_CODE_COUNT ((1<<DSSS_GOLD_CODE_NBITS) + 1)
#define DSSS_CORRELATION_LENGTH (DSSS_GOLD_CODE_LENGTH * DSSS_DECIMATION)
+/* FIXME: move to makefile */
+#define DSSS_MATCHER_CACHE_SIZE 8
+/* FIXME: move to more appropriate header */
#define PAYLOAD_DATA_BYTE ((PAYLOAD_DATA_BIT+7)/8)
struct iir_biquad {
@@ -20,22 +23,26 @@ struct cwt_iir_filter_state {
struct iir_biquad_state st[3];
};
-struct {
+struct group {
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;
+struct matcher_state {
+ int last_phase; /* 0 .. DSSS_CORRELATION_LENGTH */
int candidate_phase;
float last_score;
float candidate_score;
- uint8_t data[PAYLOAD_DATA_BYTE];
+#if DSSS_GOLD_CODE_NBITS > 7
+#error DSSS_GOLD_CODE_NBITS is too large for matcher_state.data data type (uint8_t)
+#endif
+ uint8_t data[TRANSMISSION_SYMBOLS];
int data_pos;
+ uint8_t candidate_data;
};
struct dsss_demod_state {
@@ -49,9 +56,13 @@ struct dsss_demod_state {
struct group group;
- struct group group_cache[DSSS_GROUP_CACHE_SIZE];
+ struct matcher_state matcher_cache[DSSS_MATCHER_CACHE_SIZE];
};
+
+extern void handle_dsss_received(uint8_t data[TRANSMISSION_SYMBOLS]);
+
+void dsss_demod_init(struct dsss_demod_state *st);
#ifdef SIMULATION
void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts, int record_channel);
#else /* SIMULATION */