]> Chaos Git - corbenik/ctrulib.git/commitdiff
ndsp: Monopole and biquad channel filter params
authorMerryMage <MerryMage@users.noreply.github.com>
Fri, 25 Mar 2016 18:58:00 +0000 (18:58 +0000)
committerMerryMage <MerryMage@users.noreply.github.com>
Fri, 25 Mar 2016 19:30:46 +0000 (19:30 +0000)
libctru/include/3ds/ndsp/channel.h
libctru/source/ndsp/ndsp-channel.c
libctru/source/ndsp/ndsp-filter.c [new file with mode: 0644]

index 443725c8ac3c38a0369d0a8ea85c1973b2902d3c..d3c81b49a51caa0daea09c256fb5771eba18089a 100644 (file)
@@ -167,12 +167,69 @@ void ndspChnWaveBufAdd(int id, ndspWaveBuf* buf);
  * @param enable Whether to enable the IIR monopole filter.
  */
 void ndspChnIirMonoSetEnable(int id, bool enable);
-//   ndspChnIirMonoSetParams
+/**
+ * @brief Manually sets up the parameters on monopole filter
+ * @param id ID of the channel (0..23).
+ * @param enable Whether to enable the IIR monopole filter.
+ */
+bool ndspChnIirMonoSetParamsCustomFilter(int id, float a0, float a1, float b0);
+/**
+ * @brief Sets the monopole to be a low pass filter. (Note: This is a lower-quality filter than the biquad one.)
+ * @param id ID of the channel (0..23).
+ * @param f0 Low pass cut-off frequency.
+ */
+bool ndspChnIirMonoSetParamsLowPassFilter(int id, float f0);
+/**
+ * @brief Sets the monopole to be a high pass filter. (Note: This is a lower-quality filter than the biquad one.)
+ * @param id ID of the channel (0..23).
+ * @param f0 High pass cut-off frequency.
+ */
+bool ndspChnIirMonoSetParamsHighPassFilter(int id, float f0);
 /**
  * @brief Configures whether the IIR biquad filter of a channel is enabled.
  * @param id ID of the channel (0..23).
  * @param enable Whether to enable the IIR biquad filter.
  */
 void ndspChnIirBiquadSetEnable(int id, bool enable);
-//   ndspChnIirBiquadSetParams
+/**
+ * @brief Manually sets up the parameters of the biquad filter
+ * @param id ID of the channel (0..23).
+ */
+bool ndspChnIirBiquadSetParamsCustomFilter(int id, float a0, float a1, float a2, float b0, float b1, float b2);
+/**
+ * @brief Sets the biquad to be a low pass filter.
+ * @param id ID of the channel (0..23).
+ * @param f0 Low pass cut-off frequency.
+ * @param Q "Quality factor", typically should be sqrt(2)/2 (i.e. 0.7071).
+ */
+bool ndspChnIirBiquadSetParamsLowPassFilter(int id, float f0, float Q);
+/**
+ * @brief Sets the biquad to be a high pass filter.
+ * @param id ID of the channel (0..23).
+ * @param f0 High pass cut-off frequency.
+ * @param Q "Quality factor", typically should be sqrt(2)/2 (i.e. 0.7071).
+ */
+bool ndspChnIirBiquadSetParamsHighPassFilter(int id, float f0, float Q);
+/**
+ * @brief Sets the biquad to be a band pass filter.
+ * @param id ID of the channel (0..23).
+ * @param f0 Mid-frequency.
+ * @param Q "Quality factor", typically should be sqrt(2)/2 (i.e. 0.7071).
+ */
+bool ndspChnIirBiquadSetParamsBandPassFilter(int id, float f0, float Q);
+/**
+ * @brief Sets the biquad to be a notch filter.
+ * @param id ID of the channel (0..23).
+ * @param f0 Notch frequency.
+ * @param Q "Quality factor", typically should be sqrt(2)/2 (i.e. 0.7071).
+ */
+bool ndspChnIirBiquadSetParamsNotchFilter(int id, float f0, float Q);
+/**
+ * @brief Sets the biquad to be a peaking equalizer.
+ * @param id ID of the channel (0..23).
+ * @param f0 Central frequency.
+ * @param Q "Quality factor", typically should be sqrt(2)/2 (i.e. 0.7071).
+ * @param gain Amount of gain (raw value = 10 ^ dB/40)
+ */
+bool ndspChnIirBiquadSetParamsPeakingEqualizer(int id, float f0, float Q, float gain);
 ///@}
index d9437eeb4af0db4793975eee32e03110fcac578d..3a491e963124ee95d820b7b71272351e24c01fb2 100644 (file)
@@ -11,6 +11,8 @@ enum
        CFLAG_RATE          = BIT(5),
        CFLAG_MIX           = BIT(6),
        CFLAG_ADPCMCOEFS    = BIT(7),
+       CFLAG_IIRMONO       = BIT(8),
+       CFLAG_IIRBIQUAD     = BIT(9),
 };
 
 typedef struct
@@ -25,7 +27,11 @@ typedef struct
        u16 wavBufCount, wavBufIdNext;
 
        bool playing, paused;
-       u8 interpType, iirFilterType;
+       u8 interpType;
+
+       u8 iirFilterType;
+       s16 iirMono[2];
+       s16 iirBiquad[5];
 
        u16 format;
        u16 wavBufSeq;
@@ -202,6 +208,66 @@ void ndspChnIirBiquadSetEnable(int id, bool enable)
        LightLock_Unlock(&chn->lock);
 }
 
+static s16 iirParamClamp(float param, float scale_factor, bool* success)
+{
+       float scaled = param * scale_factor;
+       s16 result = (s16) scaled;
+       if (scaled > 0x7FFF)
+       {
+               result = 0x7FFF;
+               *success = false;
+       }
+       else if (scaled < -0x8000)
+       {
+               result = -0x8000;
+               *success = false;
+       }
+       return result;
+}
+
+bool ndspChnIirMonoSetParamsCustomFilter(int id, float a0, float a1, float b0)
+{
+       bool success = true;
+       s16 params[2];
+       params[0] = iirParamClamp(+b0 / a0, (float)(1 << 15), &success);
+       params[1] = iirParamClamp(-a1 / a0, (float)(1 << 15), &success);
+
+       ndspChnSt* chn = &ndspChn[id];
+       LightLock_Lock(&chn->lock);
+
+       memcpy(chn->iirMono, params, sizeof(chn->iirMono));
+       chn->iirFilterType |= BIT(0);
+
+       chn->flags |= CFLAG_IIRMONO | CFLAG_IIRFILTERTYPE;
+
+       LightLock_Unlock(&chn->lock);
+
+       return success;
+}
+
+bool ndspChnIirBiquadSetParamsCustomFilter(int id, float a0, float a1, float a2, float b0, float b1, float b2)
+{
+       bool success = true;
+       s16 params[5];
+       params[0] = iirParamClamp(-a2 / a0, (float)(1 << 14), &success);
+       params[1] = iirParamClamp(-a1 / a0, (float)(1 << 14), &success);
+       params[2] = iirParamClamp(+b2 / a0, (float)(1 << 14), &success);
+       params[3] = iirParamClamp(+b1 / a0, (float)(1 << 14), &success);
+       params[4] = iirParamClamp(+b0 / a0, (float)(1 << 14), &success);
+
+       ndspChnSt* chn = &ndspChn[id];
+       LightLock_Lock(&chn->lock);
+
+       memcpy(chn->iirBiquad, params, sizeof(chn->iirBiquad));
+       chn->iirFilterType |= BIT(1);
+
+       chn->flags |= CFLAG_IIRBIQUAD | CFLAG_IIRFILTERTYPE;
+
+       LightLock_Unlock(&chn->lock);
+
+       return success;
+}
+
 void ndspiInitChn(void)
 {
        int i;
@@ -278,6 +344,18 @@ void ndspiUpdateChn(void)
                        stflags |= 4;
                }
 
+               if (flags & CFLAG_IIRBIQUAD)
+               {
+                       memcpy(st->iirFilter_biquad, chn->iirBiquad, sizeof(chn->iirBiquad));
+                       stflags |= 0x1000000;
+               }
+
+               if (flags & CFLAG_IIRMONO)
+               {
+                       memcpy(st->iirFilter_mono, chn->iirMono, sizeof(chn->iirMono));
+                       stflags |= 0x800000;
+               }
+
                // Do wavebuf stuff
                int wvcount = chn->wavBufCount;
                ndspWaveBuf* wb = chn->waveBuf;
diff --git a/libctru/source/ndsp/ndsp-filter.c b/libctru/source/ndsp/ndsp-filter.c
new file mode 100644 (file)
index 0000000..81a5608
--- /dev/null
@@ -0,0 +1,103 @@
+#include <math.h>
+#include <3ds/types.h>
+#include <3ds/ndsp/ndsp.h>
+#include <3ds/ndsp/channel.h>
+
+#define Fs 32728.0f
+
+bool ndspChnIirMonoSetParamsLowPassFilter(int id, float f0)
+{
+       const float w0 = 2.f * M_PI * f0 / Fs;
+
+       const float a0 = 1.f;
+       const float a1 = 1.f - expf(-w0);
+       const float b0 = expf(-w0);
+
+       return ndspChnIirMonoSetParamsCustomFilter(id, a0, a1, b0);
+}
+
+bool ndspChnIirMonoSetParamsHighPassFilter(int id, float f0)
+{
+       const float w0 = 2.f * M_PI * (0.5f - f0 / Fs);
+
+       const float a0 = 1.f;
+       const float a1 = 1.f - expf(-w0);
+       const float b0 = -expf(-w0);
+
+       return ndspChnIirMonoSetParamsCustomFilter(id, a0, a1, b0);
+}
+
+bool ndspChnIirBiquadSetParamsLowPassFilter(int id, float f0, float Q)
+{
+       const float w0 = 2.f * M_PI * f0 / Fs;
+       const float a = sinf(w0) / (2.f * Q);
+
+       const float a0 = 1.f + a;
+       const float a1 = -2.f * cosf(w0);
+       const float a2 = 1.f - a;
+       const float b0 = 0.5f * (1.f - cosf(w0));
+       const float b1 =        (1.f - cosf(w0));
+       const float b2 = 0.5f * (1.f - cosf(w0));
+
+       return ndspChnIirBiquadSetParamsCustomFilter(id, a0, a1, a2, b0, b1, b2);
+}
+
+bool ndspChnIirBiquadSetParamsHighPassFilter(int id, float f0, float Q)
+{
+       const float w0 = 2.f * M_PI * f0 / Fs;
+       const float a = sinf(w0) / (2.f * Q);
+
+       const float a0 = 1.f + a;
+       const float a1 = -2.f * cosf(w0);
+       const float a2 = 1.f - a;
+       const float b0 = 0.5f * (1.f + cosf(w0));
+       const float b1 =       -(1.f + cosf(w0));
+       const float b2 = 0.5f * (1.f + cosf(w0));
+
+       return ndspChnIirBiquadSetParamsCustomFilter(id, a0, a1, a2, b0, b1, b2);
+}
+
+bool ndspChnIirBiquadSetParamsBandPassFilter(int id, float f0, float Q)
+{
+       const float w0 = 2.f * M_PI * f0 / Fs;
+       const float a = sinf(w0) / (2.f * Q);
+
+       const float a0 = 1.f + a;
+       const float a1 = -2.f * cosf(w0);
+       const float a2 = 1.f - a;
+       const float b0 = a;
+       const float b1 = 0.f;
+       const float b2 = a;
+
+       return ndspChnIirBiquadSetParamsCustomFilter(id, a0, a1, a2, b0, b1, b2);
+}
+
+bool ndspChnIirBiquadSetParamsNotchFilter(int id, float f0, float Q)
+{
+       const float w0 = 2.f * M_PI * f0 / Fs;
+       const float a = sinf(w0) / (2.f * Q);
+
+       const float a0 = 1.f + a;
+       const float a1 = -2.f * cosf(w0);
+       const float a2 = 1.f - a;
+       const float b0 = 1.f;
+       const float b1 = -2.f * cosf(w0);
+       const float b2 = 1.f;
+
+       return ndspChnIirBiquadSetParamsCustomFilter(id, a0, a1, a2, b0, b1, b2);
+}
+
+bool ndspChnIirBiquadSetParamsPeakingEqualizer(int id, float f0, float Q, float gain)
+{
+       const float w0 = 2.f * M_PI * f0 / Fs;
+       const float a = sinf(w0) / (2.f * Q);
+
+       const float a0 = 1.f + a*gain;
+       const float a1 = -2.f * cosf(w0);
+       const float a2 = 1.f - a*gain;
+       const float b0 = 1.f + a*gain;
+       const float b1 = -2.f * cosf(w0);
+       const float b2 = 1.f - a*gain;
+
+       return ndspChnIirBiquadSetParamsCustomFilter(id, a0, a1, a2, b0, b1, b2);
+}