jaseg/safety-reset
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

demod wip

Browse source
This commit is contained in:
jaseg 2020-03-09 13:23:35 +01:00
parent b4d5293d04
commit 9debe084fc
5 changed files with 289 additions and 60 deletions
Download patch file Download diff file Expand all files Collapse all files

4
controller/fw/Makefile
View file

@ -57,9 +57,9 @@ 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
TRANSMISSION_SYMBOLS ?= 32
CC := $(PREFIX)gcc
CXX := $(PREFIX)g++
@ -105,8 +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)
COMMON_CFLAGS += -DTRANSMISSION_SYMBOLS=$(TRANSMISSION_SYMBOLS)
# for musl
CFLAGS += -Dhidden=

184
controller/fw/src/dsss_demod.c
View file

@ -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) {
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 */
}
/* 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));
}
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) {
static_assert(group_phase_tolerance > 10); /* FIXME debug, remove */
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 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;
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) {
/* Otherwise check weakest entry */
float score = score_group(&cache[i]);
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;
}
}
/* Return weakest group if weaker than candidate */
if (min_score < score_group(g))
return min_idx;
/* 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;
}
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;
/* 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]) {

23
controller/fw/src/dsss_demod.h
View file

@ -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 */

12
controller/fw/tools/dsss_demod_test.c
View file

@ -12,6 +12,16 @@
#include "dsss_demod.h"
void handle_dsss_received(uint8_t data[TRANSMISSION_SYMBOLS]) {
printf("data sequence received: [ ");
for (size_t i=0; i<TRANSMISSION_SYMBOLS; i++) {
printf("%+3d", ((data[i]&1) ? 1 : -1) * (data[i]>>1));
if (i+1 < TRANSMISSION_SYMBOLS)
printf(", ");
}
printf(" ]\n");
}
void print_usage() {
fprintf(stderr, "Usage: dsss_demod_test [test_data.bin] [optional recording channel number]\n");
}
@ -87,7 +97,7 @@ int main(int argc, char **argv) {
fprintf(stderr, "Starting simulation.\n");
struct dsss_demod_state demod;
memset(&demod, 0, sizeof(demod));
dsss_demod_init(&demod);
for (size_t i=0; i<n_samples; i++) {
//fprintf(stderr, "Iteration %zd/%zd\n", i, n_samples);
dsss_demod_step(&demod, buf_f[i], i, record_channel);

126
lab-windows/dsss_experiments-ber.ipynb
View file

@ -2,7 +2,7 @@
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"execution_count": 121,
"metadata": {},
"outputs": [],
"source": [
@ -14,11 +14,11 @@
"from collections import defaultdict\n",
"import json\n",
"\n",
"\n",
"from matplotlib import pyplot as plt\n",
"import matplotlib\n",
"import numpy as np\n",
"from scipy import signal as sig\n",
"from scipy import fftpack as fftpack\n",
"import ipywidgets\n",
"\n",
"from tqdm.notebook import tqdm\n",
@ -29,7 +29,7 @@
},
{
"cell_type": "code",
"execution_count": 2,
"execution_count": 108,
"metadata": {},
"outputs": [],
"source": [
@ -38,7 +38,7 @@
},
{
"cell_type": "code",
"execution_count": 3,
"execution_count": 109,
"metadata": {},
"outputs": [],
"source": [
@ -47,7 +47,7 @@
},
{
"cell_type": "code",
"execution_count": 4,
"execution_count": 110,
"metadata": {},
"outputs": [],
"source": [
@ -73,7 +73,7 @@
},
{
"cell_type": "code",
"execution_count": 19,
"execution_count": 111,
"metadata": {},
"outputs": [],
"source": [
@ -93,7 +93,7 @@
},
{
"cell_type": "code",
"execution_count": 6,
"execution_count": 112,
"metadata": {},
"outputs": [],
"source": [
@ -107,7 +107,7 @@
},
{
"cell_type": "code",
"execution_count": 7,
"execution_count": 113,
"metadata": {},
"outputs": [
{
@ -127,7 +127,7 @@
},
{
"cell_type": "code",
"execution_count": 21,
"execution_count": 114,
"metadata": {},
"outputs": [],
"source": [
@ -142,7 +142,7 @@
},
{
"cell_type": "code",
"execution_count": 26,
"execution_count": 115,
"metadata": {},
"outputs": [],
"source": [
@ -162,7 +162,67 @@
},
{
"cell_type": "code",
"execution_count": 9,
"execution_count": 125,
"metadata": {},
"outputs": [
{
"data": {
"application/vnd.jupyter.widget-view+json": {
"model_id": "4330b0f8ceea4d5d922d2063a81554ca",
"version_major": 2,
"version_minor": 0
},
"text/plain": [
"Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"fig, ax = plt.subplots()\n",
"ax.psd(colorednoise.powerlaw_psd_gaussian(1, 1000))\n",
"None"
]
},
{
"cell_type": "code",
"execution_count": 130,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"start 14880 end 24800 rec 29760\n"
]
}
],
"source": [
"test_duration = 32\n",
"test_nbits = 5\n",
"test_signal_amplitude=2.0e-3\n",
"test_decimation=10\n",
"test_signal_amplitude = 200e-3\n",
"noise_level = 10e-3\n",
"\n",
"#test_data = np.random.RandomState(seed=0).randint(0, 2 * (2**test_nbits), test_duration)\n",
"#test_data = np.array([0, 1, 2, 3] * 50)\n",
"test_data = np.array(range(test_duration))\n",
"signal = np.repeat(modulate(test_data, test_nbits, pad=False) * 2.0 - 1, test_decimation) * test_signal_amplitude\n",
"noise = colorednoise.powerlaw_psd_gaussian(1, len(signal)*3) * noise_level\n",
"noise[int(1.5*len(signal)):][:len(signal)] += signal\n",
"print('start', int(1.5*len(signal)), 'end', int(1.5*len(signal))+len(signal), 'rec', len(noise))\n",
"\n",
"with open(f'dsss_test_signals/dsss_test_noiseless_padded.bin', 'wb') as f:\n",
" for e in noise:\n",
" f.write(struct.pack('<f', e))"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [],
"source": [
@ -176,13 +236,13 @@
},
{
"cell_type": "code",
"execution_count": 10,
"execution_count": 7,
"metadata": {},
"outputs": [
{
"data": {
"application/vnd.jupyter.widget-view+json": {
"model_id": "373401133cfe408aa15738e48c58dfaa",
"model_id": "",
"version_major": 2,
"version_minor": 0
},
@ -198,6 +258,46 @@
"plot_distance_func()"
]
},
{
"cell_type": "code",
"execution_count": 104,
"metadata": {},
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"<ipython-input-104-abeb28a85dfa>:5: RuntimeWarning: More than 20 figures have been opened. Figures created through the pyplot interface (`matplotlib.pyplot.figure`) are retained until explicitly closed and may consume too much memory. (To control this warning, see the rcParam `figure.max_open_warning`).\n",
" fig, ax = plt.subplots()\n"
]
},
{
"data": {
"application/vnd.jupyter.widget-view+json": {
"model_id": "",
"version_major": 2,
"version_minor": 0
},
"text/plain": [
"Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"#nonlinear_distance_impl = lambda x: np.exp(-np.abs(x)/10) * x**4\n",
"nonlinear_distance_impl = lambda x: np.exp(-((x/10 - 0.5)%1 - 0.5)**2 / (2*1.2/10**2))\n",
"\n",
"def plot_distance_func_impl():\n",
" fig, ax = plt.subplots()\n",
" x = np.linspace(-30, 30, 10000)\n",
" ax.plot(x, nonlinear_distance_impl(x))\n",
"\n",
"plot_distance_func_impl()"
]
},
{
"cell_type": "code",
"execution_count": 11,