Working on DSSS demodulator sim
This commit is contained in:
jaseg
parent
d9b26d16c0
commit
4b419bd1ad
14 changed files with 94312 additions and 62 deletions
180
controller/fw/src/dsss_demod.c
Normal file
180
controller/fw/src/dsss_demod.c
Normal file
|
|
@ -0,0 +1,180 @@
|
|||
|
||||
#include <unistd.h>
|
||||
#include <stdbool.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <arm_math.h>
|
||||
|
||||
#include "freq_meas.h"
|
||||
#include "sr_global.h"
|
||||
#include "dsss_demod.h"
|
||||
#include "simulation.h"
|
||||
|
||||
#include "generated/dsss_gold_code.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}},
|
||||
};
|
||||
|
||||
float gold_correlate_step(const size_t ncode, const float a[DSSS_CORRELATION_LENGTH], size_t offx);
|
||||
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);
|
||||
|
||||
#ifdef SIMULATION
|
||||
void dsss_demod_step(struct dsss_demod_state *st, float new_value, size_t sim_pos) {
|
||||
#else /* SIMULATION */
|
||||
void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
|
||||
#endif /* SIMULATION */
|
||||
|
||||
//#define DEBUG_PRINT(...) ((void)0)
|
||||
//#define DEBUG_PRINTN(...) ((void)0)
|
||||
bool debug = sim_pos % DSSS_CORRELATION_LENGTH == 0;
|
||||
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;
|
||||
|
||||
st->signal[st->signal_wpos] = new_value;
|
||||
st->signal_wpos = (st->signal_wpos + 1) % ARRAY_LENGTH(st->signal);
|
||||
if (debug) DEBUG_PRINT(" signal: %f", 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);
|
||||
/* 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]);
|
||||
|
||||
float cwt[DSSS_GOLD_CODE_COUNT];
|
||||
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];
|
||||
for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++)
|
||||
avg[i] = run_iir(fabs(cwt[i]), ARRAY_LENGTH(cwt_filter_bq), cwt_filter_bq, st->cwt_filter[i].st);
|
||||
/* debug */
|
||||
/*
|
||||
DEBUG_PRINTN(" avg: [");
|
||||
for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++)
|
||||
DEBUG_PRINTN("%f, ", avg[i]);
|
||||
DEBUG_PRINTN("]\n");
|
||||
*/
|
||||
/* end */
|
||||
|
||||
float max_val = st->group.max;
|
||||
int max_ch = st->group.max_ch;
|
||||
int max_idx = st->group.max_idx + 1;
|
||||
bool found = false;
|
||||
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 (fabs(val) > DSSS_THESHOLD_FACTOR)
|
||||
found = true;
|
||||
|
||||
if (fabs(val) > fabs(max_val)) {
|
||||
max_val = val;
|
||||
max_ch = i;
|
||||
max_idx = st->group.len;
|
||||
}
|
||||
}
|
||||
if (debug) DEBUG_PRINTN("]\n");
|
||||
|
||||
/* debug */
|
||||
if (debug) DEBUG_PRINT(" found=%d len=%d idx=%d ch=%d max=%f",
|
||||
found, st->group.len, st->group.max_idx, st->group.max_ch, st->group.max);
|
||||
/* end */
|
||||
|
||||
if (found) {
|
||||
/* Continue ongoing group */
|
||||
st->group.len++;
|
||||
st->group.max = max_val;
|
||||
st->group.max_ch = max_ch;
|
||||
st->group.max_idx = max_idx;
|
||||
return;
|
||||
}
|
||||
|
||||
if (st->group.len == 0)
|
||||
/* We're between groups */
|
||||
return;
|
||||
|
||||
/* A group ended. Process result. */
|
||||
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);
|
||||
|
||||
/* reset grouping state */
|
||||
st->group.len = 0;
|
||||
st->group.max_idx = 0;
|
||||
st->group.max_ch = 0;
|
||||
st->group.max = 0.0f;
|
||||
}
|
||||
|
||||
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++)
|
||||
intermediate = run_biquad(intermediate, &q[i], &st[i]);
|
||||
return intermediate;
|
||||
}
|
||||
|
||||
float run_biquad(float x, const struct iir_biquad *const q, struct iir_biquad_state *const restrict st) {
|
||||
/* direct form 2, see https://en.wikipedia.org/wiki/Digital_biquad_filter */
|
||||
float intermediate = x + st->reg[0] * -q->a[0] + st->reg[1] * -q->a[1];
|
||||
float out = intermediate * q->b[0] + st->reg[0] * q->b[1] + st->reg[1] * q->b[2];
|
||||
st->reg[1] = st->reg[0];
|
||||
st->reg[0] = intermediate;
|
||||
return out;
|
||||
}
|
||||
|
||||
float cwt_convolve_step(const float v[DSSS_WAVELET_LUT_SIZE], size_t offx) {
|
||||
float sum = 0.0f;
|
||||
for (ssize_t j=0; j<DSSS_WAVELET_LUT_SIZE; j++) {
|
||||
/* Our wavelet is symmetric so convolution and correlation are identical. Use correlation here for ease of
|
||||
* implementation */
|
||||
sum += v[(offx + j) % DSSS_WAVELET_LUT_SIZE] * dsss_cwt_wavelet_table[j];
|
||||
//DEBUG_PRINT(" j=%d v=%f w=%f", j, v[(offx + j) % DSSS_WAVELET_LUT_SIZE], dsss_cwt_wavelet_table[j]);
|
||||
}
|
||||
return sum / DSSS_WAVELET_LUT_SIZE;
|
||||
}
|
||||
|
||||
/* Compute last element of correlation for input [a] and hard-coded gold sequences.
|
||||
*
|
||||
* This is intened to be used once for each new incoming sample in [a]. It expects [a] to be of length
|
||||
* [dsss_correlation_length] and produces the one sample where both the reference sequence and the input fully overlap.
|
||||
* This is equivalent to "valid" mode in numpy's terminology[0].
|
||||
*
|
||||
* [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 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 */
|
||||
}
|
||||
acc_outer += acc_inner;
|
||||
}
|
||||
return acc_outer / DSSS_CORRELATION_LENGTH;
|
||||
}
|
||||
44
controller/fw/src/dsss_demod.h
Normal file
44
controller/fw/src/dsss_demod.h
Normal file
|
|
@ -0,0 +1,44 @@
|
|||
#ifndef __DSSS_DEMOD_H__
|
||||
#define __DSSS_DEMOD_H__
|
||||
|
||||
#define DSSS_GOLD_CODE_LENGTH ((1<<DSSS_GOLD_CODE_NBITS) - 1)
|
||||
#define DSSS_GOLD_CODE_COUNT ((1<<DSSS_GOLD_CODE_NBITS) + 1)
|
||||
#define DSSS_CORRELATION_LENGTH (DSSS_GOLD_CODE_LENGTH * DSSS_DECIMATION)
|
||||
|
||||
struct iir_biquad {
|
||||
float a[2];
|
||||
float b[3];
|
||||
};
|
||||
|
||||
struct iir_biquad_state {
|
||||
float reg[2];
|
||||
};
|
||||
|
||||
struct cwt_iir_filter_state {
|
||||
struct iir_biquad_state st[3];
|
||||
};
|
||||
|
||||
struct dsss_demod_state {
|
||||
float signal[DSSS_CORRELATION_LENGTH];
|
||||
size_t signal_wpos;
|
||||
|
||||
float correlation[DSSS_GOLD_CODE_COUNT][DSSS_WAVELET_LUT_SIZE];
|
||||
size_t correlation_wpos;
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
#ifdef SIMULATION
|
||||
void dsss_demod_step(struct dsss_demod_state *st, float new_value, size_t sim_pos);
|
||||
#else /* SIMULATION */
|
||||
void dsss_demod_step(struct dsss_demod_state *st, float new_value);
|
||||
#endif /* SIMULATION */
|
||||
|
||||
#endif /* __DSSS_DEMOD_H__ */
|
||||
|
|
@ -2,6 +2,7 @@
|
|||
#define __SR_GLOBAL_H__
|
||||
|
||||
#define UNUSED(x) ((void) x)
|
||||
#define ARRAY_LENGTH(x) (sizeof(x) / sizeof(x[0]))
|
||||
|
||||
static inline uint16_t htole(uint16_t val) { return val; }
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue