diff --git a/dsp/CMakeLists.txt b/dsp/CMakeLists.txt index 65f8cac..f0cce89 100644 --- a/dsp/CMakeLists.txt +++ b/dsp/CMakeLists.txt @@ -15,6 +15,7 @@ add_library(soothe2_dsp SHARED filter.cpp detect.cpp twin.cpp + freqpath.cpp phase_table.cpp ) diff --git a/dsp/freqpath.cpp b/dsp/freqpath.cpp new file mode 100644 index 0000000..a7260ce --- /dev/null +++ b/dsp/freqpath.cpp @@ -0,0 +1,28 @@ +#include "freqpath.hpp" + +#include +#include + +namespace detkernel { + +// Bit-faithful transcription of the warp builder loop in FUN_180530850 (0x530900). +void build_warp(float fs_total, int n, float* warp) { + const float K = std::exp(2.0723267f); // 0x1824c4208 -> exp (0x1a14cac) + const float F = (2000.0f / (fs_total * 0.5f)) * static_cast(n); // 0x1824c45b4 / ([0x24]*0.5) * N + + for (int i = 1; i < n; ++i) { + float x = static_cast(i) / F; + float t = x / K; + t = t + 1.0f; // +1.0 (0x1824c3ea4) + float r = 1.0f / t; // divss + r = static_cast(std::fabs(static_cast(r))); // cvtss2sd/andpd/cvtpd2ps + warp[i] = r * x; + } + + const float scale = 0.87f; // 0x1824c3e50 (also 0.87 as double 0x24c4110) + for (int i = 0; i < n; ++i) warp[i] *= scale; + + warp[0] = 0.0f; // 0x5406a8[0] = 0 (DC) +} + +} // namespace detkernel \ No newline at end of file diff --git a/dsp/freqpath.hpp b/dsp/freqpath.hpp new file mode 100644 index 0000000..9db344b --- /dev/null +++ b/dsp/freqpath.hpp @@ -0,0 +1,29 @@ +#pragma once +#include +#include + +// Freq-path (FUN_180530850) warp and resonance-vs-warp product. +// +// WARP 0x5406a8 (peak builder loop @0x530900, disasm f530850_full.dis): +// K = expf(2.0723267f) (0x1824c4208 -> call 0x1a14cac) +// F = (2000.0f / (fs_total * 0.5f)) * (float)N (0x1824c45b4 / [0x24]*0x24c3d8c, +// scaled by (count/2+1) = N) +// for i in 1..N-1: +// x = (float)i / F (bin freq i ~ f = i*sr*0.5/N -> x ~ f/2000) +// r = 1.0f / (x / K + 1.0f) (0x1824c3ea4 = 1.0) +// r = (float)fabs((double)r) (cvtss2sd + andpd 0x24c4f10 + cvtpd2ps) +// w[i] = r * x +// w[0..N) *= 0.87f (0x1824c3e50), 0x5406a8[0] = 0 (0x24c4110 = 0.87 as double) +// +// warp(500)=0.211 warp(1000)=0.409 warp(2000)=0.773 (fs=44100) +// +// FUN_180530850 then applies the twin resonance (fc=8000.0, Q=1.0, sens c42d0=3.0 +// via generator 0x180533ec0 + kernel 0x180535880) into 0x5406f8 and multiplies +// 0x5406f8 *= 0x5406a8 (thunk window-multiply 0x52d990). warp[0]=0 zeroes DC. + +namespace detkernel { + +// Build the 0x5406a8 warp (float, N bins, DC = 0). +void build_warp(float fs_total, int n, float* warp); + +} // namespace detkernel \ No newline at end of file diff --git a/dsp/twin_check.cpp b/dsp/twin_check.cpp new file mode 100644 index 0000000..1b92666 --- /dev/null +++ b/dsp/twin_check.cpp @@ -0,0 +1,108 @@ +#include +#include +#include + +#include "freqpath.hpp" +#include "twin.hpp" + +// Phase-2 gate: the float twin transcription must reproduce the double +// reference |2*B/A| (model_lut.res_at) to float rounding accuracy at the +// Phase-1 fc-scan parameters (Q=xmlq, gain=sqrt(sens_stored)=4.132). + +static double res_ref(double ft, double fc, double q, double gain) { + const double fs = 44100.0; + const double w0 = fc * 2.0 * acos(-1.0) / fs; + const double c = cos(w0), s = sin(w0); + const double p = (s * 0.5) / q; + const double a0 = 1.0 + p * gain, a2 = 1.0 - p * gain; + const double b0 = 1.0 + p / gain, b2 = 1.0 - p / gain; + const double w = 2.0 * acos(-1.0) * ft / fs; + const double zr = cos(w), zi = -sin(w); + const double ar = a0 + (-2.0 * c) * zr + a2 * (zr * zr - zi * zi); + const double ai = (-2.0 * c) * zi + a2 * 2.0 * zr * zi; + const double br = b0 + (-2.0 * c) * zr + b2 * (zr * zr - zi * zi); + const double bi = (-2.0 * c) * zi + b2 * 2.0 * zr * zi; + return 2.0 * std::sqrt((br * br + bi * bi) / (ar * ar + ai * ai)); +} + +int main() { + const double fs = 44100.0; + const double fcs[] = {800.0, 900.0, 950.0, 1000.0, 1050.0, 1100.0, 1200.0}; + const double q = 0.9999978; + const double gain = 4.132; + const float sens_lin = static_cast(gain * gain); // param_5 -> sqrtf = gain + + const double ft = 1000.0; + const detkernel::cplxf z = { + static_cast(std::cos(2.0 * acos(-1.0) * ft / fs)), + static_cast(std::sin(2.0 * acos(-1.0) * ft / fs)), + }; + + std::printf("fc |twin| |ref| rel-err 20log10(twin)\n"); + double maxrel = 0.0; + for (double fc : fcs) { + detkernel::twin_coeff c = detkernel::build_twin_coeff(fs, fc, q, sens_lin); + detkernel::cplxf out; + detkernel::twin_apply(c, &z, 1, &out); + const double mag = std::sqrt((double)out.re * out.re + (double)out.im * out.im); + const double ref = res_ref(ft, fc, q, gain); + const double rel = std::fabs(mag - ref) / ref; + if (rel > maxrel) maxrel = rel; + const double db = 20.0 * std::log10(mag); + std::printf("%5.1f %9.6f %9.6f %9.2e %9.4f\n", fc, mag, ref, rel, db); + } + std::printf("\nmax rel err = %.3e\n", maxrel); + // float32 rcpps+Newton + fma Horner cumulative parity bound (~ ulp * stages) + std::printf("PASS = %s\n", maxrel < 5e-5 ? "yes (float-parity)" : "NO"); + bool ok = maxrel < 5e-5; + + // Full operating-space sweep: DUAL grid Q=0.1..10, tones 500/2000, fc=500. + std::printf("\n--- DUAL sweep (fc=500, gain=4.132), |2B/A| rel-err ---\n"); + const double qs[] = {0.1, 0.2, 0.3, 0.5, 0.7, 1.0, 1.5, 2.0, 3.0, 5.0, 10.0}; + const double ftone[] = {500.0, 2000.0}; + const detkernel::cplxf zt[2] = { + {static_cast(std::cos(2.0 * acos(-1.0) * 500.0 / fs)), + static_cast(std::sin(2.0 * acos(-1.0) * 500.0 / fs))}, + {static_cast(std::cos(2.0 * acos(-1.0) * 2000.0 / fs)), + static_cast(std::sin(2.0 * acos(-1.0) * 2000.0 / fs))}, + }; + double msweep = 0.0; + for (double q : qs) { + detkernel::twin_coeff c = detkernel::build_twin_coeff(fs, 500.0, q, sens_lin); + for (int t = 0; t < 2; ++t) { + detkernel::cplxf out; + detkernel::twin_apply(c, &zt[t], 1, &out); + const double mag = std::sqrt((double)out.re * out.re + (double)out.im * out.im); + const double ref = res_ref(ftone[t], 500.0, q, gain); + const double rel = std::fabs(mag - ref) / ref; + if (rel > msweep) msweep = rel; + } + } + std::printf("max sweep rel err = %.3e\n", msweep); + std::printf("SWEEP PASS = %s\n", msweep < 5e-4 ? "yes" : "NO"); + ok = ok && (msweep < 5e-4); + + // Freq-path warp 0x5406a8 (FUN_180530850): 0.87*x/(1+x/K), K=exp(2.0723). + std::printf("\n--- freq-path warp 0x5406a8 (fs=44100) ---\n"); + const int N = 1025; + const float K = std::exp(2.0723267f); + std::vector warp(N); + detkernel::build_warp(44100.0f, N, warp.data()); + const double Fr = (2000.0 / 22050.0) * N; // F for fs=44100 + const double freqs[] = {500.0, 1000.0, 2000.0, 8000.0}; + double werr = 0.0; + for (double f : freqs) { + const int bin = static_cast(std::lround((N - 1) * f / 22050.0)); // real-bins mapping + const double x = static_cast(bin) / Fr; + const double wref = 0.87 * x * K / (K + x); + const double rel = std::fabs(warp[bin] - wref) / wref; + if (rel > werr) werr = rel; + std::printf("%6.0f Hz: bin=%4d x=%.4f warp=%8.6f (ref=%8.6f rel=%.2e)\n", + f, bin, x, warp[bin], wref, rel); + } + std::printf("warp max rel err = %.3e ; warp(0)=%g\n", werr, warp[0]); + std::printf("WARP PASS = %s\n", werr < 1e-6 ? "yes" : "NO"); + ok = ok && warp[0] == 0.0f; + + return ok ? 0 : 1; +} \ No newline at end of file