diff options
Diffstat (limited to 'controller/fw')
-rw-r--r-- | controller/fw/Makefile | 2 | ||||
-rw-r--r-- | controller/fw/src/dsss_demod.c | 49 | ||||
-rw-r--r-- | controller/fw/src/main.c | 17 | ||||
-rw-r--r-- | controller/fw/src/sr_global.h | 5 |
4 files changed, 47 insertions, 26 deletions
diff --git a/controller/fw/Makefile b/controller/fw/Makefile index 50d162d..0faea30 100644 --- a/controller/fw/Makefile +++ b/controller/fw/Makefile @@ -25,7 +25,7 @@ FMEAS_SAMPLING_RATE ?= $(shell echo $(FMEAS_ADC_SAMPLING_RATE) / \($(FMEAS_FFT_ DSSS_GOLD_CODE_NBITS ?= 5 DSSS_DECIMATION ?= 10 # TODO maybe auto adjust this based on detection rate? -DSSS_THESHOLD_FACTOR ?= 6.0f +DSSS_THESHOLD_FACTOR ?= 5.0f DSSS_WAVELET_WIDTH ?= 7.3 DSSS_WAVELET_LUT_SIZE ?= 69 DSSS_FILTER_FC ?= 3e-3 diff --git a/controller/fw/src/dsss_demod.c b/controller/fw/src/dsss_demod.c index 0758525..f9efda6 100644 --- a/controller/fw/src/dsss_demod.c +++ b/controller/fw/src/dsss_demod.c @@ -23,10 +23,8 @@ struct iir_biquad cwt_filter_bq[DSSS_FILTER_CLEN] = {DSSS_FILTER_COEFF}; 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); -#if 0 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); -#endif 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); @@ -41,7 +39,7 @@ void debug_print_vector(const char *name, size_t len, const float *data, size_t if (index) { DEBUG_PRINTN(" %16s [", ""); for (size_t i=0; i<len; i++) - DEBUG_PRINTN("%8zd ", i); + DEBUG_PRINTN("%8zu ", i); DEBUG_PRINTN("]\n"); } @@ -62,6 +60,8 @@ void dsss_demod_init(struct dsss_demod_state *st) { void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts) { //const float hole_patching_threshold = 0.01 * DSSS_CORRELATION_LENGTH; + bool log = false; + bool log_groups = true; st->signal[st->signal_wpos] = new_value; st->signal_wpos = (st->signal_wpos + 1) % ARRAY_LENGTH(st->signal); @@ -80,6 +80,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts) for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) avg += fabsf(cwt[i]); avg /= (float)DSSS_GOLD_CODE_COUNT; + if (log) DEBUG_PRINTN("%6zu: %f ", ts, avg); /* FIXME fix this filter */ //avg = run_iir(avg, ARRAY_LENGTH(cwt_filter_bq), cwt_filter_bq, st->cwt_filter.st); @@ -89,18 +90,21 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts) bool found = false; for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) { float val = cwt[i] / avg; + if (log) DEBUG_PRINTN("%f ", cwt[i]); + if (log) DEBUG_PRINTN("%f ", val); if (fabsf(val) > fabsf(max_val)) { max_val = val; max_ch = i; max_ts = ts; - - if (fabsf(val) > DSSS_THESHOLD_FACTOR) - found = true; } + + if (fabsf(val) > DSSS_THESHOLD_FACTOR) + found = true; } + if (log) DEBUG_PRINTN("%f %d ", max_val, found); + if (log) DEBUG_PRINTN("\n"); - /* FIXME: skipped sample handling here */ matcher_tick(st->matcher_cache, ts, max_ch, max_val); if (found) { @@ -116,6 +120,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts) /* We're between groups */ return; + if (log_groups) DEBUG_PRINTN("GROUP: %zu %d %f\n", st->group.max_ts, st->group.max_ch, st->group.max); /* A group ended. Process result. */ group_received(st); @@ -152,7 +157,9 @@ void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE], u const float score_depreciation = 0.1f; /* 0.0 -> no depreciation, 1.0 -> complete disregard */ const int current_phase = ts % DSSS_CORRELATION_LENGTH; const int max_skips = TRANSMISSION_SYMBOLS/4*3; + bool debug = true; + bool header_printed = false; for (size_t i=0; i<DSSS_MATCHER_CACHE_SIZE; i++) { if (states[i].last_phase == -1) continue; /* Inactive entry */ @@ -161,6 +168,11 @@ void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE], u /* Skip sampling */ float score = fabsf(peak_ampl) * (1.0f - skip_sampling_depreciation); if (score > states[i].candidate_score) { + if (debug && !header_printed) { + header_printed = true; + DEBUG_PRINTN("windows %zu\n", ts); + } + if (debug) DEBUG_PRINTN(" skip %d old=%f new=%f\n", i, states[i].candidate_score, score); /* We win, update candidate */ assert(i < DSSS_MATCHER_CACHE_SIZE); states[i].candidate_score = score; @@ -175,7 +187,15 @@ void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE], u * 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) { + int phase_delta = current_phase - states[i].last_phase; + if (phase_delta < 0) + phase_delta += DSSS_CORRELATION_LENGTH; + if (phase_delta == group_phase_tolerance + DSSS_DECIMATION) { + if (debug && !header_printed) { + header_printed = true; + DEBUG_PRINTN("windows %zu\n", ts); + } + if (debug) DEBUG_PRINTN(" %d ", i); /* Process window results */ assert(i < DSSS_MATCHER_CACHE_SIZE); assert(0 <= states[i].data_pos && states[i].data_pos < TRANSMISSION_SYMBOLS); @@ -183,17 +203,21 @@ void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE], u 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; + if (debug) DEBUG_PRINTN("commit pos=%d val=%d score=%f ", states[i].data_pos, states[i].candidate_data, states[i].last_score); states[i].candidate_score = 0.0f; states[i].last_skips += states[i].candidate_skips; if (states[i].last_skips > max_skips) { + if (debug) DEBUG_PRINTN("expire "); states[i].last_phase = -1; /* invalidate entry */ } else if (states[i].data_pos == TRANSMISSION_SYMBOLS) { + if (debug) DEBUG_PRINTN("match "); /* Frame received completely */ handle_dsss_received(states[i].data); states[i].last_phase = -1; /* invalidate entry */ } + if (debug) DEBUG_PRINTN("\n"); } } } @@ -211,6 +235,7 @@ static float score_group(const struct group *g, int phase_delta) { } void group_received(struct dsss_demod_state *st) { + bool debug = true; 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); @@ -234,6 +259,7 @@ void group_received(struct dsss_demod_state *st) { float group_score = score_group(&st->group, phase_delta); if (st->matcher_cache[i].candidate_score < group_score) { assert(i < DSSS_MATCHER_CACHE_SIZE); + if (debug) DEBUG_PRINTN(" appending %zu %d score=%f pd=%d\n", i, decode_peak(st->group.max_ch, st->group.max), group_score, phase_delta); /* Append to entry */ st->matcher_cache[i].candidate_score = group_score; st->matcher_cache[i].candidate_phase = group_phase; @@ -243,7 +269,8 @@ void group_received(struct dsss_demod_state *st) { } /* Search for weakest entry */ - float score = st->matcher_cache[i].last_score; + /* TODO figure out this fitness function */ + float score = st->matcher_cache[i].last_score * (1.5f + 1.0f/st->matcher_cache[i].data_pos); if (score < min_score) { min_idx = i; min_score = score; @@ -252,6 +279,7 @@ void group_received(struct dsss_demod_state *st) { /* If we found empty entries, replace one by a new decoding starting at this group */ if (empty_idx >= 0) { + if (debug) DEBUG_PRINTN(" empty %zd %d\n", empty_idx, decode_peak(st->group.max_ch, st->group.max)); assert(0 <= empty_idx && empty_idx < DSSS_MATCHER_CACHE_SIZE); st->matcher_cache[empty_idx].last_phase = group_phase; st->matcher_cache[empty_idx].candidate_score = base_score; @@ -264,6 +292,7 @@ void group_received(struct dsss_demod_state *st) { /* If the weakest decoding in cache is weaker than a new decoding starting here, replace it */ } else if (min_score < base_score && min_idx >= 0) { + if (debug) DEBUG_PRINTN(" min %zd %d\n", min_idx, decode_peak(st->group.max_ch, st->group.max)); assert(0 <= min_idx && min_idx < DSSS_MATCHER_CACHE_SIZE); st->matcher_cache[min_idx].last_phase = group_phase; st->matcher_cache[min_idx].candidate_score = base_score; @@ -276,7 +305,6 @@ void group_received(struct dsss_demod_state *st) { } } -#if 0 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++) @@ -292,7 +320,6 @@ float run_biquad(float x, const struct iir_biquad *const q, struct iir_biquad_st st->reg[0] = intermediate; return out; } -#endif float cwt_convolve_step(const float v[DSSS_WAVELET_LUT_SIZE], size_t offx) { float sum = 0.0f; diff --git a/controller/fw/src/main.c b/controller/fw/src/main.c index 6589297..814c723 100644 --- a/controller/fw/src/main.c +++ b/controller/fw/src/main.c @@ -216,23 +216,14 @@ int main(void) con_printf("Booted.\r\n"); while (23) { if (adc_fft_buf_ready_idx != -1) { + for (int i=0; i<168*1000*2; i++) + asm volatile ("nop"); GPIOA->BSRR = 1<<11; - //adc_fft_buf_ready_idx = !adc_fft_buf_ready_idx; /* DEBUG */ - //DEBUG: - //memcpy(adc_fft_buf[!adc_fft_buf_ready_idx], adc_fft_buf[adc_fft_buf_ready_idx] + FMEAS_FFT_LEN/2, sizeof(adc_fft_buf[0][0]) * FMEAS_FFT_LEN/2); + memcpy(adc_fft_buf[!adc_fft_buf_ready_idx], adc_fft_buf[adc_fft_buf_ready_idx] + FMEAS_FFT_LEN/2, sizeof(adc_fft_buf[0][0]) * FMEAS_FFT_LEN/2); + GPIOA->BSRR = 1<<11<<16; for (int i=0; i<168*1000*2; i++) asm volatile ("nop"); - /* BEGIN DEBUG */ - con_printf_blocking("\r\n%06d: ", freq_sample_ts); - int old_idx = adc_fft_buf_ready_idx; - for (int i=0; i<FMEAS_FFT_LEN/2; i++) - con_printf_blocking("%03x ", adc_fft_buf[old_idx][FMEAS_FFT_LEN/2 + i]); - adc_fft_buf_ready_idx = -1; - freq_sample_ts++; /* TODO: also increase in case of freq measurement error? */ - GPIOA->BSRR = 1<<11<<16; - continue; - /* END DEBUG */ GPIOA->BSRR = 1<<11; float out; diff --git a/controller/fw/src/sr_global.h b/controller/fw/src/sr_global.h index 451cce0..97db4e4 100644 --- a/controller/fw/src/sr_global.h +++ b/controller/fw/src/sr_global.h @@ -2,9 +2,12 @@ #define __SR_GLOBAL_H__ #include <stdint.h> +#include <sys/types.h> + +#ifndef SIMULATION #include <stm32f407xx.h> #include <stm32f4_isr.h> -#include <sys/types.h> +#endif #define UNUSED(x) ((void) x) #define ARRAY_LENGTH(x) (sizeof(x) / sizeof(x[0])) |