fleg
/
OpenRTX
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

core: dsp: refactor DC block filter implementation 

New DC block filter implementation using fixed-point math and guaranteeing
zero DC component on the output signal.

Signed-off-by: Silvano Seva <silseva@fastwebnet.it>
This commit is contained in:
Silvano Seva 2025-09-26 21:05:14 +02:00
parent 4b9f75fe81
commit 6db558e89c
7 changed files with 65 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
openrtx/include/core/dsp.h
View File

@ -29,41 +29,49 @@
extern "C" {
#endif
typedef int16_t audio_sample_t;
/*
* This header contains various DSP utilities which can be used to condition
* input or output signals when implementing digital modes on OpenRTX.
*/
/**
* Data structure holding the internal state of a filter.
*/
typedef struct
{
float u[3]; // input values u(k), u(k-1), u(k-2)
float y[3]; // output values y(k), y(k-1), y(k-2)
bool initialised; // state variables initialised
}
filter_state_t;
/**
* Reset the filter state variables.
* Reset the state variables of a DSP object.
*
* @param state: pointer to the data structure containing the filter state.
* @param state: state variable.
*/
void dsp_resetFilterState(filter_state_t *state);
#define dsp_resetState(state) memset(&state, 0x00, sizeof(state))
/**
* Remove the DC offset from a collection of audio samples, processing data
* in-place.
* Data structure holding the internal state of a DC blocking filter.
*/
struct dcBlock {
int32_t accum;
int32_t prevIn;
int32_t prevOut;
};
/**
* Run a single step of the DC blocking filter.
*
* @param dcb: pointer to DC filter state.
* @param sample: input sample.
* @return filtered sample
*/
int16_t dsp_dcBlockFilter(struct dcBlock *dcb, int16_t sample);
/**
* Remove the DC offset from a collection of samples, processing data in-place.
*
* @param state: pointer to the data structure containing the filter state.
* @param buffer: buffer containing the audio samples.
* @param length: number of samples contained in the buffer.
*/
void dsp_dcRemoval(filter_state_t *state, audio_sample_t *buffer, size_t length);
static inline void dsp_removeDcOffset(struct dcBlock *dcb, int16_t *buffer,
const size_t length)
{
for (size_t i = 0; i < length; i++)
buffer[i] = dsp_dcBlockFilter(dcb, buffer[i]);
}
/*
* Inverts the phase of the audio buffer passed as paramenter.

2
openrtx/include/protocols/M17/M17Demodulator.hpp
View File

@ -162,7 +162,7 @@ private:
uint32_t syncCount; ///< Downcounter for resynchronization
std::pair < int32_t, int32_t > outerDeviation; ///< Deviation of outer symbols
float corrThreshold; ///< Correlation threshold
filter_state_t dcrState; ///< State of the DC removal filter
struct dcBlock dcBlock; ///< State of the DC removal filter
Correlator < M17_SYNCWORD_SYMBOLS, SAMPLES_PER_SYMBOL > correlator;
Synchronizer < M17_SYNCWORD_SYMBOLS, SAMPLES_PER_SYMBOL > streamSync{{ -3, -3, -3, -3, +3, +3, -3, +3 }};

22
openrtx/src/core/audio_codec.c
View File

@ -54,12 +54,10 @@ static uint8_t numElements;
static uint64_t dataBuffer[BUF_SIZE];
#ifdef PLATFORM_MOD17
static const uint8_t micGainPre = 4;
static const uint8_t micGainPost = 3;
static const uint8_t micGain = 12;
#else
#ifndef PLATFORM_LINUX
static const uint8_t micGainPre = 8;
static const uint8_t micGainPost = 4;
static const uint8_t micGain = 32;
#endif
#endif
@ -194,7 +192,7 @@ static void *encodeFunc(void *arg)
pathId iPath = *((pathId*) arg);
stream_sample_t audioBuf[320];
struct CODEC2 *codec2;
filter_state_t dcrState;
struct dcBlock dcBlock;
iStream = audioStream_start(iPath, audioBuf, 320, 8000,
STREAM_INPUT | BUF_CIRC_DOUBLE);
@ -205,7 +203,7 @@ static void *encodeFunc(void *arg)
return NULL;
}
dsp_resetFilterState(&dcrState);
dsp_resetState(dcBlock);
codec2 = codec2_create(CODEC2_MODE_3200);
while(reqStop == false)
@ -219,14 +217,12 @@ static void *encodeFunc(void *arg)
break;
#ifndef PLATFORM_LINUX
// Pre-amplification stage
for(size_t i = 0; i < audio.len; i++) audio.data[i] *= micGainPre;
for(size_t i = 0; i < audio.len; i++) {
int16_t sample;
// DC removal
dsp_dcRemoval(&dcrState, audio.data, audio.len);
// Post-amplification stage
for(size_t i = 0; i < audio.len; i++) audio.data[i] *= micGainPost;
sample = dsp_dcBlockFilter(&dcBlock, audio.data[i]);
audio.data[i] = sample * micGain;
}
#endif
// CODEC2 encodes 160ms of speech into 8 bytes: here we write the

53
openrtx/src/core/dsp.cpp
View File

@ -20,51 +20,22 @@
#include <dsp.h>
void dsp_resetFilterState(filter_state_t *state)
{
state->u[0] = 0.0f;
state->u[1] = 0.0f;
state->u[2] = 0.0f;
state->y[0] = 0.0f;
state->y[1] = 0.0f;
state->y[2] = 0.0f;
state->initialised = false;
}
void dsp_dcRemoval(filter_state_t *state, audio_sample_t *buffer, size_t length)
int16_t dsp_dcBlockFilter(struct dcBlock *dcb, int16_t sample)
{
/*
* Removal of DC component performed using an high-pass filter with
* transfer function G(z) = (z - 1)/(z - 0.999).
* Recursive implementation of the filter is:
* y(k) = u(k) - u(k-1) + 0.999*y(k-1)
* Implementation of a fixed-point DC block filter with noise shaping,
* ensuring zero DC component at the output.
* Filter pole set at 0.995
*
* Code adapted from https://dspguru.com/dsp/tricks/fixed-point-dc-blocking-filter-with-noise-shaping/
*/
dcb->accum -= dcb->prevIn;
dcb->prevIn = static_cast<int32_t>(sample) << 15;
dcb->accum += dcb->prevIn;
dcb->accum -= 164 * dcb->prevOut; // 32768.0 * (1.0 - pole)
dcb->prevOut = dcb->accum >> 15;
if(length < 2) return;
static constexpr float alpha = 0.999f;
size_t pos = 0;
if(state->initialised == false)
{
state->u[1] = static_cast< float >(buffer[0]);
state->initialised = true;
pos = 1;
}
for(; pos < length; pos++)
{
state->u[0] = static_cast< float >(buffer[pos]);
state->y[0] = (state->u[0])
- (state->u[1])
+ alpha * (state->y[1]);
state->u[1] = state->u[0];
state->y[1] = state->y[0];
buffer[pos] = static_cast< audio_sample_t >(state->y[0] + 0.5f);
}
return static_cast<int16_t>(dcb->prevOut);
}
void dsp_invertPhase(audio_sample_t *buffer, uint16_t length)

12
openrtx/src/protocols/M17/M17Demodulator.cpp
View File

@ -237,16 +237,16 @@ bool M17Demodulator::update(const bool invertPhase)
dataBlock_t baseband = inputStream_getData(basebandId);
if(baseband.data != NULL)
{
// Apply DC removal filter
dsp_dcRemoval(&dcrState, baseband.data, baseband.len);
// Process samples
for(size_t i = 0; i < baseband.len; i++)
{
// Apply DC removal filter
int16_t sample = dsp_dcBlockFilter(&dcBlock, baseband.data[i]);
// Apply RRC on the baseband sample
float elem = static_cast< float >(baseband.data[i]);
float elem = static_cast< float >(sample);
if(invertPhase) elem = 0.0f - elem;
int16_t sample = static_cast< int16_t >(M17::rrc_24k(elem));
sample = static_cast< int16_t >(M17::rrc_24k(elem));
// Update correlator and sample filter for correlation thresholds
correlator.sample(sample);
@ -429,7 +429,7 @@ void M17Demodulator::reset()
demodState = DemodState::INIT;
initCount = RX_SAMPLE_RATE / 50; // 50ms of init time
dsp_resetFilterState(&dcrState);
dsp_resetState(dcBlock);
}

2
scripts/clang_format.sh
View File

@ -53,6 +53,7 @@ openrtx/include/core/audio_codec.h
openrtx/include/core/battery.h
openrtx/include/core/beeps.h
openrtx/include/core/crc.h
openrtx/include/core/dsp.h
openrtx/include/core/data_conversion.h
openrtx/include/core/datatypes.h
openrtx/include/core/memory_profiling.h
@ -68,6 +69,7 @@ openrtx/include/interfaces/radio.h
openrtx/include/peripherals/gps.h
openrtx/include/peripherals/rng.h
openrtx/include/peripherals/rtc.h
openrtx/src/core/dsp.cpp
openrtx/src/core/memory_profiling.cpp
platform/drivers/ADC/ADC0_GDx.h
platform/drivers/audio/MAX9814.h

19
tests/platform/mic_test.c
View File

@ -20,6 +20,7 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <interfaces/delays.h>
#include <interfaces/audio.h>
#include <audio_path.h>
@ -31,8 +32,8 @@ int main()
{
platform_init();
filter_state_t dcrState;
dsp_resetFilterState(&dcrState);
struct dcBlock dcb;
dsp_resetState(dcb);
static const size_t numSamples = 45 * 1024; // 90kB
void *sampleBuf = malloc(numSamples * sizeof(stream_sample_t));
@ -51,16 +52,10 @@ int main()
platform_ledOn(RED);
sleepFor(10u, 0u);
// Pre-processing gain
for (size_t i = 0; i < audio.len; i++)
audio.data[i] <<= 3;
// DC removal
dsp_dcRemoval(&dcrState, audio.data, audio.len);
// Post-processing gain
for (size_t i = 0; i < audio.len; i++)
audio.data[i] *= 10;
for (size_t i = 0; i < audio.len; i++) {
int16_t sample = dsp_dcBlockFilter(&dcb, audio.data[i]);
audio.data[i] = sample * 32;
}
uint16_t *ptr = ((uint16_t *)audio.data);
for (size_t i = 0; i < audio.len; i++)