add RPLL port

This commit is contained in:
Sebastien Bourdeauducq 2023-08-28 22:24:58 +08:00
parent 1c54151f31
commit d95f780674
1 changed files with 103 additions and 0 deletions

103
dsplib.cpp Normal file
View File

@ -0,0 +1,103 @@
#include <cstdint>
#include <tuple>
#include <optional>
#include <cassert>
// C++ port of RPLL from https://github.com/quartiq/idsp
// MIT or Apache-2.0 license
/// Reciprocal PLL.
///
/// Consumes noisy, quantized timestamps of a reference signal and reconstructs
/// the phase and frequency of the update() invocations with respect to (and in units of
/// 1 << 32 of) that reference.
/// In other words, `update()` rate ralative to reference frequency,
/// `u32::MAX` corresponding to both being equal.
class RPLL {
private:
uint32_t dt2; // 1 << dt2 is the counter rate to update() rate ratio
int32_t x; // previous timestamp
uint32_t ff; // current frequency estimate from frequency loop
uint32_t f; // current frequency estimate from both frequency and phase loop
int32_t y; // current phase estimate
public:
RPLL(uint32_t dt2);
std::tuple<int32_t, uint32_t> update(std::optional<int32_t>, uint32_t, uint32_t);
int32_t phase();
uint32_t frequency();
};
/// Create a new RPLL instance.
///
/// Args:
/// * dt2: inverse update() rate. 1 << dt2 is the counter rate to update() rate ratio.
///
/// Returns:
/// Initialized RPLL instance.
RPLL::RPLL(uint32_t dt2):
dt2(dt2),
x(0),
ff(0),
f(0),
y(0)
{}
/// Advance the RPLL and optionally supply a new timestamp.
///
/// Args:
/// * input: Optional new timestamp (wrapping around at the i32 boundary).
/// There can be at most one timestamp per `update()` cycle (1 << dt2 counter cycles).
/// * shift_frequency: Frequency lock settling time. 1 << shift_frequency is
/// frequency lock settling time in counter periods. The settling time must be larger
/// than the signal period to lock to.
/// * shift_phase: Phase lock settling time. Usually one less than
/// `shift_frequency` (see there).
///
/// Returns:
/// A tuple containing the current phase (wrapping at the i32 boundary, pi) and
/// frequency.
std::tuple<int32_t, uint32_t> RPLL::update(std::optional<int32_t> input,
uint32_t shift_frequency,
uint32_t shift_phase)
{
assert(shift_frequency >= dt2);
assert(shift_phase >= dt2);
// Advance phase
y += f;
if(input.has_value()) {
// Reference period in counter cycles
int32_t dx = *input - x;
// Store timestamp for next time
x = *input;
// Phase using the current frequency estimate
uint64_t p_sig_64 = (uint64_t)ff * (uint64_t)dx;
// Add half-up rounding bias and apply gain/attenuation
uint32_t p_sig =
(p_sig_64 + (1 << (shift_frequency - 1))) >> shift_frequency;
// Reference phase (1 << dt2 full turns) with gain/attenuation applied
uint32_t p_ref = 1 << (32 + dt2 - shift_frequency);
// Update frequency lock
ff += p_ref - p_sig;
// Time in counter cycles between timestamp and "now"
uint32_t dt = -x & ((1 << dt2) - 1);
// Reference phase estimate "now"
int32_t y_ref = (f >> dt2)*dt;
// Phase error with gain
int32_t dy = (y_ref - y) >> (shift_phase - dt2);
// Current frequency estimate from frequency lock and phase error
f = ff + dy;
}
return { y, f };
}
/// Return the current phase estimate
int32_t RPLL::phase()
{
return y;
}
/// Return the current frequency estimate
uint32_t RPLL::frequency()
{
return f;
}