155 lines
3.9 KiB
C
155 lines
3.9 KiB
C
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <malloc.h>
|
|
#include <math.h>
|
|
|
|
#include "freq_est.h"
|
|
|
|
#include "utils.h"
|
|
|
|
#include "correlator.h"
|
|
|
|
bool correlator_init(correlator_ctx_t *ctx, const float complex *search_seq, size_t nsymbols)
|
|
{
|
|
// copy arguments and initialize variables
|
|
ctx->search_sequence_len = nsymbols;
|
|
|
|
ctx->input_history = NULL;
|
|
ctx->search_sequence = NULL;
|
|
|
|
ctx->history_ptr = 0;
|
|
|
|
// Determine the smallest power of two greater than nsymbols
|
|
ctx->buffer_size = 1;
|
|
while(nsymbols > 0) {
|
|
ctx->buffer_size <<= 1;
|
|
nsymbols >>= 1;
|
|
}
|
|
|
|
// If buffer_size = 0b1000, then buffer_size_mask=0b0111
|
|
ctx->buffer_size_mask = ctx->buffer_size - 1;
|
|
|
|
// Allocate the buffers
|
|
ctx->input_history = malloc(ctx->buffer_size * sizeof(ctx->input_history[0]));
|
|
if(!ctx->input_history) {
|
|
fprintf(stderr, "correlator: malloc() failed!\n");
|
|
goto fail;
|
|
}
|
|
|
|
ctx->search_sequence = malloc(ctx->buffer_size * sizeof(ctx->search_sequence[0]));
|
|
if(!ctx->search_sequence) {
|
|
fprintf(stderr, "correlator: malloc() failed!\n");
|
|
goto fail;
|
|
}
|
|
|
|
// Clear the history buffer
|
|
memset(ctx->input_history, 0, ctx->buffer_size * sizeof(ctx->input_history[0]));
|
|
|
|
// Prepare the search sequence.
|
|
// The complex values are conjugated in the process.
|
|
for(size_t i = 0; i < ctx->search_sequence_len; i++) {
|
|
ctx->search_sequence[i] = conjf(search_seq[i]);
|
|
}
|
|
|
|
// Success!
|
|
return true;
|
|
|
|
fail:
|
|
correlator_free(ctx);
|
|
return false;
|
|
}
|
|
|
|
|
|
void correlator_free(correlator_ctx_t *ctx)
|
|
{
|
|
if(ctx->input_history) {
|
|
free(ctx->input_history);
|
|
ctx->input_history = NULL;
|
|
}
|
|
|
|
if(ctx->search_sequence) {
|
|
free(ctx->search_sequence);
|
|
ctx->input_history = NULL;
|
|
}
|
|
|
|
ctx->buffer_size = 0;
|
|
ctx->search_sequence_len = 0;
|
|
}
|
|
|
|
|
|
float complex correlator_step(correlator_ctx_t *ctx, float complex sample)
|
|
{
|
|
float complex result = 0;
|
|
|
|
// increment and wrap the history pointer
|
|
ctx->history_ptr++;
|
|
ctx->history_ptr &= ctx->buffer_size_mask;
|
|
|
|
// copy the input sample to the history
|
|
ctx->input_history[ctx->history_ptr] = sample;
|
|
|
|
size_t n = (ctx->history_ptr - ctx->search_sequence_len + 1) & ctx->buffer_size_mask;
|
|
|
|
// calculate the correlation
|
|
for(size_t m = 0; m < ctx->search_sequence_len; m++) {
|
|
size_t nm = (n + m) & ctx->buffer_size_mask;
|
|
|
|
result += ctx->search_sequence[m] * ctx->input_history[nm];
|
|
}
|
|
|
|
// the current mean phase can be basically calculated for free here, as the
|
|
// sum of rotated symbols is already available, so we do so and cache that
|
|
// value.
|
|
ctx->phase_deviation = cargf(result);
|
|
|
|
return result;
|
|
}
|
|
|
|
|
|
void correlator_get_input_history(correlator_ctx_t *ctx, float complex *history)
|
|
{
|
|
size_t n = (ctx->history_ptr - ctx->search_sequence_len + 1) & ctx->buffer_size_mask;
|
|
|
|
for(size_t m = 0; m < ctx->search_sequence_len; m++) {
|
|
size_t nm = (n + m) & ctx->buffer_size_mask;
|
|
history[m] = ctx->input_history[nm];
|
|
}
|
|
}
|
|
|
|
|
|
const float complex* correlator_get_conj_search_sequence(correlator_ctx_t *ctx)
|
|
{
|
|
return ctx->search_sequence;
|
|
}
|
|
|
|
|
|
float correlator_get_mean_phase_deviation(correlator_ctx_t *ctx)
|
|
{
|
|
// return cached value from last call to correlator_step().
|
|
return ctx->phase_deviation;
|
|
}
|
|
|
|
|
|
float correlator_get_mean_frequency_deviation(correlator_ctx_t *ctx, size_t L, float *phase_offset)
|
|
{
|
|
float complex z[L];
|
|
|
|
size_t n = (ctx->history_ptr - ctx->search_sequence_len + 1) & ctx->buffer_size_mask;
|
|
|
|
// remove the influence of the data from the received symbols
|
|
size_t m0 = ctx->search_sequence_len - L;
|
|
for(size_t m = m0; m < ctx->search_sequence_len; m++) {
|
|
size_t nm = (n + m) & ctx->buffer_size_mask;
|
|
z[m - m0] = conjf(ctx->search_sequence[m]) * ctx->input_history[nm];
|
|
}
|
|
|
|
float freq = freq_est_data_free(z, L, NULL);
|
|
|
|
// we calculate the final phase based on the phase estimated from the
|
|
// preamble and not the phase from the frequency estimator because this
|
|
// method is more reliable.
|
|
*phase_offset = ctx->phase_deviation + (ctx->search_sequence_len+1)/2.0f * freq;
|
|
|
|
return freq;
|
|
}
|