mirror of
https://github.com/portapack-mayhem/mayhem-firmware.git
synced 2026-08-27 09:59:03 +00:00
Belousov Oleg
233 lines
9.3 KiB
C++
233 lines
9.3 KiB
C++
/*
|
|
* Copyright (C) 2026
|
|
*
|
|
* This file is part of PortaPack.
|
|
*/
|
|
|
|
#ifndef __DSP_FREQUENCY_XLATOR_H__
|
|
#define __DSP_FREQUENCY_XLATOR_H__
|
|
|
|
#include "dsp_decimate.hpp"
|
|
#include "dsp_types.hpp"
|
|
|
|
#include <array>
|
|
#include <cstdint>
|
|
|
|
namespace dsp {
|
|
|
|
/* Fixed-point complex mixer for the Audio RX channelizer. */
|
|
class FrequencyTranslator {
|
|
public:
|
|
FrequencyTranslator() {
|
|
for (size_t i = 0; i < oscillator_q15_.size(); ++i) {
|
|
oscillator_q15_[i] =
|
|
static_cast<uint16_t>(sine_q15_[static_cast<uint8_t>(i + 64)]) |
|
|
(static_cast<uint32_t>(
|
|
static_cast<uint16_t>(sine_q15_[i]))
|
|
<< 16);
|
|
}
|
|
}
|
|
|
|
void set_sample_rate(const uint32_t sampling_rate) {
|
|
sampling_rate_ = sampling_rate;
|
|
update_phase_increment();
|
|
}
|
|
|
|
void set_frequency(const int32_t frequency) {
|
|
frequency_ = frequency;
|
|
update_phase_increment();
|
|
}
|
|
|
|
buffer_c16_t execute(const buffer_c16_t& src, const buffer_c16_t& dst) {
|
|
auto phase = phase_;
|
|
for (size_t i = 0; i < src.count; ++i) {
|
|
const uint8_t index = phase >> 24;
|
|
const uint8_t next = index + 1;
|
|
const int32_t fraction = (phase >> 16) & 0xff;
|
|
const uint32_t oscillator_first = oscillator_q15_[index];
|
|
const uint32_t oscillator_next = oscillator_q15_[next];
|
|
const int32_t sine_first =
|
|
static_cast<int16_t>(oscillator_first >> 16);
|
|
const int32_t cosine_first =
|
|
static_cast<int16_t>(oscillator_first);
|
|
const int32_t sine =
|
|
sine_first +
|
|
(((static_cast<int16_t>(oscillator_next >> 16) -
|
|
sine_first) *
|
|
fraction) >>
|
|
8);
|
|
const int32_t cosine =
|
|
cosine_first +
|
|
(((static_cast<int16_t>(oscillator_next) -
|
|
cosine_first) *
|
|
fraction) >>
|
|
8);
|
|
const uint32_t oscillator =
|
|
static_cast<uint16_t>(cosine) |
|
|
(static_cast<uint32_t>(
|
|
static_cast<uint16_t>(sine))
|
|
<< 16);
|
|
const uint32_t sample =
|
|
*reinterpret_cast<const uint32_t*>(&src.p[i]);
|
|
|
|
/* Two packed dual-16-bit multiplies implement
|
|
* (I+jQ) * (cos-j sin). */
|
|
const int32_t out_i =
|
|
rounded_shift(__SMUAD(sample, oscillator), 15);
|
|
const int32_t out_q =
|
|
rounded_shift(__SMUSDX(oscillator, sample), 15);
|
|
*reinterpret_cast<uint32_t*>(&dst.p[i]) =
|
|
__PKHBT(__SSAT(out_i, 16), __SSAT(out_q, 16), 16);
|
|
phase += phase_increment_;
|
|
}
|
|
phase_ = phase;
|
|
return {dst.p, src.count, src.sampling_rate};
|
|
}
|
|
|
|
private:
|
|
friend class FrequencyTranslatingDecimator32By8;
|
|
|
|
static constexpr int32_t rounded_shift(
|
|
const int32_t value,
|
|
const uint32_t bits) {
|
|
const int32_t rounding = int32_t{1} << (bits - 1);
|
|
return value >= 0
|
|
? (value + rounding) >> bits
|
|
: -((-value + rounding) >> bits);
|
|
}
|
|
|
|
void update_phase_increment() {
|
|
if (sampling_rate_) {
|
|
phase_increment_ = static_cast<uint32_t>(
|
|
(static_cast<int64_t>(frequency_) * (int64_t{1} << 32)) /
|
|
sampling_rate_);
|
|
}
|
|
}
|
|
|
|
static constexpr std::array<int16_t, 256> sine_q15_{{
|
|
0, 804, 1608, 2410, 3212, 4011, 4808, 5602, 6393, 7179, 7962, 8739, 9512, 10278, 11039, 11793,
|
|
12539, 13279, 14010, 14732, 15446, 16151, 16846, 17530, 18204, 18868, 19519, 20159, 20787, 21403, 22005, 22594,
|
|
23170, 23731, 24279, 24811, 25329, 25832, 26319, 26790, 27245, 27683, 28105, 28510, 28898, 29268, 29621, 29956,
|
|
30273, 30571, 30852, 31113, 31356, 31580, 31785, 31971, 32137, 32285, 32412, 32521, 32609, 32678, 32728, 32757,
|
|
32767, 32757, 32728, 32678, 32609, 32521, 32412, 32285, 32137, 31971, 31785, 31580, 31356, 31113, 30852, 30571,
|
|
30273, 29956, 29621, 29268, 28898, 28510, 28105, 27683, 27245, 26790, 26319, 25832, 25329, 24811, 24279, 23731,
|
|
23170, 22594, 22005, 21403, 20787, 20159, 19519, 18868, 18204, 17530, 16846, 16151, 15446, 14732, 14010, 13279,
|
|
12539, 11793, 11039, 10278, 9512, 8739, 7962, 7179, 6393, 5602, 4808, 4011, 3212, 2410, 1608, 804,
|
|
0, -804, -1608, -2410, -3212, -4011, -4808, -5602, -6393, -7179, -7962, -8739, -9512, -10278, -11039, -11793,
|
|
-12539, -13279, -14010, -14732, -15446, -16151, -16846, -17530, -18204, -18868, -19519, -20159, -20787, -21403, -22005, -22594,
|
|
-23170, -23731, -24279, -24811, -25329, -25832, -26319, -26790, -27245, -27683, -28105, -28510, -28898, -29268, -29621, -29956,
|
|
-30273, -30571, -30852, -31113, -31356, -31580, -31785, -31971, -32137, -32285, -32412, -32521, -32609, -32678, -32728, -32757,
|
|
-32767, -32757, -32728, -32678, -32609, -32521, -32412, -32285, -32137, -31971, -31785, -31580, -31356, -31113, -30852, -30571,
|
|
-30273, -29956, -29621, -29268, -28898, -28510, -28105, -27683, -27245, -26790, -26319, -25832, -25329, -24811, -24279, -23731,
|
|
-23170, -22594, -22005, -21403, -20787, -20159, -19519, -18868, -18204, -17530, -16846, -16151, -15446, -14732, -14010, -13279,
|
|
-12539, -11793, -11039, -10278, -9512, -8739, -7962, -7179, -6393, -5602, -4808, -4011, -3212, -2410, -1608, -804,
|
|
}};
|
|
|
|
std::array<uint32_t, 256> oscillator_q15_{};
|
|
uint32_t phase_{0};
|
|
uint32_t phase_increment_{0};
|
|
uint32_t sampling_rate_{192000};
|
|
int32_t frequency_{0};
|
|
};
|
|
|
|
/*
|
|
* Frequency-translating 32-tap FIR decimator. Frequency translation is
|
|
* split between coefficients modulated when tuning changes and a cheap
|
|
* output-rate phase rotation. This avoids running an NCO at the 384kHz
|
|
* input rate.
|
|
*/
|
|
class FrequencyTranslatingDecimator32By8 {
|
|
public:
|
|
static constexpr size_t decimation_factor = 8;
|
|
static constexpr size_t taps_count = 32;
|
|
|
|
void configure(
|
|
const std::array<int16_t, taps_count>& taps,
|
|
const uint32_t input_sampling_rate) {
|
|
taps_ = taps;
|
|
input_sampling_rate_ = input_sampling_rate;
|
|
decimator_.configure(complex_taps_, decimation_factor);
|
|
output_xlator_.set_sample_rate(input_sampling_rate / decimation_factor);
|
|
update_taps();
|
|
}
|
|
|
|
void set_frequency(const int32_t frequency) {
|
|
frequency_ = frequency;
|
|
output_xlator_.set_frequency(frequency);
|
|
update_taps();
|
|
}
|
|
|
|
buffer_c16_t execute(
|
|
const buffer_c16_t& src,
|
|
const buffer_c16_t& dst) {
|
|
const auto filtered = decimator_.execute(src, dst);
|
|
return output_xlator_.execute(filtered, dst);
|
|
}
|
|
|
|
private:
|
|
static void oscillator(
|
|
const uint32_t phase,
|
|
int32_t& sine,
|
|
int32_t& cosine) {
|
|
const uint8_t index = phase >> 24;
|
|
const uint8_t next = index + 1;
|
|
const uint8_t cosine_index = index + 64;
|
|
const uint8_t cosine_next = cosine_index + 1;
|
|
const int32_t fraction = (phase >> 16) & 0xff;
|
|
const int32_t sine_first = FrequencyTranslator::sine_q15_[index];
|
|
const int32_t cosine_first = FrequencyTranslator::sine_q15_[cosine_index];
|
|
sine = sine_first +
|
|
(((FrequencyTranslator::sine_q15_[next] - sine_first) * fraction) >> 8);
|
|
cosine = cosine_first +
|
|
(((FrequencyTranslator::sine_q15_[cosine_next] - cosine_first) * fraction) >> 8);
|
|
}
|
|
|
|
void update_taps() {
|
|
if (!input_sampling_rate_)
|
|
return;
|
|
|
|
const uint32_t tap_phase_increment = static_cast<uint32_t>(
|
|
(static_cast<int64_t>(frequency_) * (int64_t{1} << 32)) /
|
|
input_sampling_rate_);
|
|
|
|
/* Centre the modulation on the FIR midpoint. Besides changing
|
|
* only a constant output phase, this makes the two coefficients
|
|
* in each symmetric pair complex conjugates. That property is
|
|
* important after quantization: starting at tap zero accumulated
|
|
* a one-sided phase and rounding error across the whole filter. */
|
|
uint32_t phase = static_cast<uint32_t>(
|
|
-((static_cast<int64_t>(
|
|
static_cast<int32_t>(tap_phase_increment)) *
|
|
static_cast<int64_t>(taps_count - 1)) /
|
|
2));
|
|
for (size_t i = 0; i < taps_count; ++i) {
|
|
int32_t sine;
|
|
int32_t cosine;
|
|
oscillator(phase, sine, cosine);
|
|
/* FIRAndDecimateComplex uses Q16 coefficients; the source
|
|
* real-tap filters use Q15 coefficients. */
|
|
const int32_t tap = taps_[i];
|
|
complex_taps_[i] = {
|
|
static_cast<int16_t>(
|
|
FrequencyTranslator::rounded_shift(
|
|
tap * cosine, 14)),
|
|
static_cast<int16_t>(
|
|
FrequencyTranslator::rounded_shift(
|
|
tap * sine, 14))};
|
|
phase += tap_phase_increment;
|
|
}
|
|
decimator_.set_taps(complex_taps_);
|
|
}
|
|
|
|
std::array<int16_t, taps_count> taps_{};
|
|
std::array<complex16_t, taps_count> complex_taps_{};
|
|
decimate::FIRAndDecimateComplex decimator_{};
|
|
FrequencyTranslator output_xlator_{};
|
|
uint32_t input_sampling_rate_{0};
|
|
int32_t frequency_{0};
|
|
};
|
|
|
|
} /* namespace dsp */
|
|
|
|
#endif /*__DSP_FREQUENCY_XLATOR_H__*/
|