diff --git a/openrtx/include/core/dsp.h b/openrtx/include/core/dsp.h index 4c4a3c45..fda797b4 100644 --- a/openrtx/include/core/dsp.h +++ b/openrtx/include/core/dsp.h @@ -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. diff --git a/openrtx/include/protocols/M17/M17Demodulator.hpp b/openrtx/include/protocols/M17/M17Demodulator.hpp index 78201059..1fc1e85d 100644 --- a/openrtx/include/protocols/M17/M17Demodulator.hpp +++ b/openrtx/include/protocols/M17/M17Demodulator.hpp @@ -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 }}; diff --git a/openrtx/src/core/audio_codec.c b/openrtx/src/core/audio_codec.c index b5cc5c6a..adb8b432 100644 --- a/openrtx/src/core/audio_codec.c +++ b/openrtx/src/core/audio_codec.c @@ -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 diff --git a/openrtx/src/core/dsp.cpp b/openrtx/src/core/dsp.cpp index 35a15fc6..5f36684c 100644 --- a/openrtx/src/core/dsp.cpp +++ b/openrtx/src/core/dsp.cpp @@ -20,51 +20,22 @@ #include -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(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(dcb->prevOut); } void dsp_invertPhase(audio_sample_t *buffer, uint16_t length) diff --git a/openrtx/src/protocols/M17/M17Demodulator.cpp b/openrtx/src/protocols/M17/M17Demodulator.cpp index b919b446..888e1a67 100644 --- a/openrtx/src/protocols/M17/M17Demodulator.cpp +++ b/openrtx/src/protocols/M17/M17Demodulator.cpp @@ -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); } diff --git a/scripts/clang_format.sh b/scripts/clang_format.sh index 7c8e946d..a48a54b2 100755 --- a/scripts/clang_format.sh +++ b/scripts/clang_format.sh @@ -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 diff --git a/tests/platform/mic_test.c b/tests/platform/mic_test.c index 9e6d6217..52050a99 100644 --- a/tests/platform/mic_test.c +++ b/tests/platform/mic_test.c @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -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++)