diff options
Diffstat (limited to 'controller')
-rw-r--r-- | controller/fw/Makefile | 7 | ||||
-rw-r--r-- | controller/fw/src/dsss_demod.c | 64 | ||||
-rw-r--r-- | controller/fw/src/dsss_demod.h | 33 | ||||
-rw-r--r-- | controller/fw/tools/butter_filter_gen.py | 35 |
4 files changed, 117 insertions, 22 deletions
diff --git a/controller/fw/Makefile b/controller/fw/Makefile index 3b9cca6..c549e47 100644 --- a/controller/fw/Makefile +++ b/controller/fw/Makefile @@ -52,11 +52,14 @@ FMEAS_SAMPLING_RATE ?= 10.0 DSSS_GOLD_CODE_NBITS ?= 5 DSSS_DECIMATION ?= 10 # TODO maybe auto adjust this based on detection rate? -DSSS_THESHOLD_FACTOR ?= 5.0f +DSSS_THESHOLD_FACTOR ?= 6.0f DSSS_WAVELET_WIDTH ?= 7.3 DSSS_WAVELET_LUT_SIZE ?= 69 DSSS_FILTER_FC ?= 3e-3 DSSS_FILTER_ORDER ?= 12 +DSSS_GROUP_CACHE_SIZE ?= 12 + +PAYLOAD_DATA_BIT ?= 64 CC := $(PREFIX)gcc CXX := $(PREFIX)g++ @@ -102,6 +105,8 @@ COMMON_CFLAGS += -DDSSS_DECIMATION=$(DSSS_DECIMATION) COMMON_CFLAGS += -DDSSS_THESHOLD_FACTOR=$(DSSS_THESHOLD_FACTOR) COMMON_CFLAGS += -DDSSS_WAVELET_WIDTH=$(DSSS_WAVELET_WIDTH) COMMON_CFLAGS += -DDSSS_WAVELET_LUT_SIZE=$(DSSS_WAVELET_LUT_SIZE) +COMMON_CFLAGS += -DDSSS_GROUP_CACHE_SIZE=$(DSSS_GROUP_CACHE_SIZE) +COMMON_CFLAGS += -DPAYLOAD_DATA_BIT=$(PAYLOAD_DATA_BIT) # for musl CFLAGS += -Dhidden= 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__ */ diff --git a/controller/fw/tools/butter_filter_gen.py b/controller/fw/tools/butter_filter_gen.py index c77a6ec..0bb81bc 100644 --- a/controller/fw/tools/butter_filter_gen.py +++ b/controller/fw/tools/butter_filter_gen.py @@ -18,11 +18,21 @@ def wrap(left='{', right='}', file=None, end=''): def print_include_guards(macro_name): print(f'#ifndef {macro_name}') print(f'#define {macro_name}') + print() yield + print() print(f'#endif /* {macro_name} */') macro_float = lambda f: f'{f}'.replace('.', 'F').replace('-', 'N').replace('+', 'P') +ordinal = lambda n: "%d%s" % (n,"tsnrhtdd"[(n//10%10!=1)*(n%10<4)*n%10::4]) + +SI_TABLE = {-18: 'a', -15: 'f', -12: 'p', -9: 'n', -6: 'ยต', -3: 'm', 0: '', 3: 'k', 6: 'M', 9: 'G', 12: 'T', 15: 'P', 18: 'E'} +def siprefix(x, space=' ', unit=''): + l = math.log10(x)//3*3 + if l in SI_TABLE: + return f'{x/10**l}{space}{SI_TABLE[l]}{unit}' + return f'{x}{space}{unit}' if __name__ == '__main__': import argparse @@ -36,10 +46,17 @@ if __name__ == '__main__': sos = sig.butter(args.n, args.fc, fs=args.fs, output='sos') print('/* THIS IS A GENERATED FILE. DO NOT EDIT! */') + print() with print_include_guards(f'__BUTTER_FILTER_GENERATED_{args.n}_{macro_float(args.fc)}_{macro_float(args.fs)}__'): + + print(f'/* {ordinal(args.n)} order Butterworth IIR filter coefficients') + print(f' *') + print(f' * corner frequency f_c = {siprefix(args.fc)}Hz') + print(f' * sampling rate f_s = {siprefix(args.fs)}Hz') + print(f' */') + print() print(f'#define {args.macro_name.upper()}_ORDER {args.n}') print(f'#define {args.macro_name.upper()}_CLEN {(args.n+1)//2}') - print(f'#define {args.macro_name.upper()}_COEFF ', end='') # scipy.signal.butter by default returns extremely small bs for the first biquad and large ones for subsequent # sections. Balance magnitudes to reduce possible rounding errors. @@ -48,10 +65,14 @@ if __name__ == '__main__': mags = [approx_mag // len(sos)] * len(sos) mags[0] += approx_mag - sum(mags) sos[0][:3] /= 10**approx_mag + sos = np.array([ sec * np.array([10**mag, 10**mag, 10**mag, 1, 1, 1]) for mag, sec in zip(mags, sos) ]) + + ones = np.ones([100000]) + _, steady_state = sig.sosfilt(sos, ones, zi=np.zeros([(args.n+1)//2, 2])) - for mag, sec in zip(mags, sos): + print(f'#define {args.macro_name.upper()}_COEFF ', end='') + for sec in sos: bs, ases = sec[:3], sec[4:6] - bs *= 10**mag with wrap(): print('.b=', end='') @@ -62,3 +83,11 @@ if __name__ == '__main__': print(', '.join(f'{v}' for v in ases), end='') print(', ', end='') print() + + print(f'#define {args.macro_name.upper()}_STEADY_STATE ', end='') + for sec in steady_state: + with wrap(): + print(', '.join(f'{v}' for v in sec), end='') + print(', ', end='') + print() + |