Add AM spectrum ZOOM x3/x4 with optimized x4 decimation (#3304)

* Add AM spectrum zoom x3 and x4
* Optimize AM spectrum zoom x4 processing
* Fix AM spectrum zoom formatting
* Fix AM spectrum zoom header formatting

firmware/baseband/dsp_decimate.cpp

@@ -337,6 +337,59 @@ buffer_c16_t FIRC16xR16x16Decim2::execute(
         src.sampling_rate / decimation_factor};
 }
 
+// FIRC16xR16x32Decim4 ////////////////////////////////////////////////////
+
+void FIRC16xR16x32Decim4::configure(
+    const std::array<tap_t, taps_count>& taps,
+    const int32_t scale) {
+    std::copy(taps.cbegin(), taps.cend(), taps_.begin());
+    output_scale = scale;
+    z_.fill({});
+}
+
+buffer_c16_t FIRC16xR16x32Decim4::execute(
+    const buffer_c16_t& src,
+    const buffer_c16_t& dst) {
+    vec2_s16* const z = static_cast<vec2_s16*>(__builtin_assume_aligned(z_.data(), 4));
+    const vec2_s16* const t = static_cast<vec2_s16*>(__builtin_assume_aligned(taps_.data(), 4));
+    uint32_t* const d = static_cast<uint32_t*>(__builtin_assume_aligned(dst.p, 4));
+
+    const auto k = output_scale;
+    const size_t count = src.count / decimation_factor;
+
+    for (size_t i = 0; i < count; i++) {
+        const vec2_s16* const in = static_cast<const vec2_s16*>(__builtin_assume_aligned(&src.p[i * decimation_factor], 4));
+
+        complex32_t accum;
+
+        accum = mac_shift(z, t, 0, accum);
+        accum = mac_shift(z, t, 1, accum);
+
+        accum = mac_shift_and_store(z, t, decimation_factor, 0, accum);
+        accum = mac_shift_and_store(z, t, decimation_factor, 1, accum);
+        accum = mac_shift_and_store(z, t, decimation_factor, 2, accum);
+        accum = mac_shift_and_store(z, t, decimation_factor, 3, accum);
+        accum = mac_shift_and_store(z, t, decimation_factor, 4, accum);
+        accum = mac_shift_and_store(z, t, decimation_factor, 5, accum);
+        accum = mac_shift_and_store(z, t, decimation_factor, 6, accum);
+        accum = mac_shift_and_store(z, t, decimation_factor, 7, accum);
+        accum = mac_shift_and_store(z, t, decimation_factor, 8, accum);
+        accum = mac_shift_and_store(z, t, decimation_factor, 9, accum);
+        accum = mac_shift_and_store(z, t, decimation_factor, 10, accum);
+        accum = mac_shift_and_store(z, t, decimation_factor, 11, accum);
+
+        accum = mac_shift_and_store_new_c16_samples(z, t, in, decimation_factor, 0, taps_count, accum);
+        accum = mac_shift_and_store_new_c16_samples(z, t, in, decimation_factor, 1, taps_count, accum);
+
+        d[i] = scale_round_and_pack(accum, k);
+    }
+
+    return {
+        dst.p,
+        count,
+        src.sampling_rate / decimation_factor};
+}
+
 // FIRC16xR16x63HalfbandDecim2 ////////////////////////////////////////////
 
 void FIRC16xR16x63HalfbandDecim2::configure(

firmware/baseband/dsp_decimate.hpp

@@ -170,6 +170,28 @@ class FIRC16xR16x16Decim2 {
     int32_t output_scale = 0;
 };
 
+class FIRC16xR16x32Decim4 {
+   public:
+    static constexpr size_t taps_count = 32;
+    static constexpr size_t decimation_factor = 4;
+
+    using sample_t = complex16_t;
+    using tap_t = int16_t;
+
+    void configure(
+        const std::array<tap_t, taps_count>& taps,
+        const int32_t scale = c16_to_c32_sat_scalar);
+
+    buffer_c16_t execute(
+        const buffer_c16_t& src,
+        const buffer_c16_t& dst);
+
+   private:
+    std::array<vec2_s16, taps_count - decimation_factor> z_{};
+    std::array<tap_t, taps_count> taps_{};
+    int32_t output_scale = 0;
+};
+
 class FIRC16xR16x63HalfbandDecim2 {
    public:
     static constexpr size_t taps_count = 63;

firmware/baseband/filtered_spectrum_collector.cpp

@@ -12,8 +12,22 @@
 #include "dsp_fir_taps.hpp"
 #include "event_m4.hpp"
 
+#include "ch.h"
+
 #include <algorithm>
 
+namespace {
+
+constexpr bool valid_decimation(
+    const size_t decimation_factor,
+    const size_t maximum_decimation) {
+    return decimation_factor >= 2 &&
+           decimation_factor <= maximum_decimation &&
+           (decimation_factor & (decimation_factor - 1)) == 0;
+}
+
+}  // namespace
+
 void FilteredSpectrumCollector::on_message(const Message* const message) {
     if (message->id == Message::ID::UpdateSpectrum) {
         update();
@@ -21,11 +35,17 @@ void FilteredSpectrumCollector::on_message(const Message* const message) {
     SpectrumCollector::on_message(message);
 }
 
-void FilteredSpectrumCollector::start_capture(
+bool FilteredSpectrumCollector::start_capture(
     const size_t decimation_factor) {
+    if (!valid_decimation(decimation_factor, maximum_decimation)) {
+        capture_ready_ = false;
+        return false;
+    }
+
     capture_decimation_ = decimation_factor;
     capture_count_ = 0;
     capture_ready_ = false;
+    return true;
 }
 
 bool FilteredSpectrumCollector::feed(
@@ -38,7 +58,7 @@ bool FilteredSpectrumCollector::feed(
         filter_high_frequency,
         filter_transition);
 
-    const size_t required_samples = 256 * capture_decimation_;
+    const size_t required_samples = fft_samples * capture_decimation_;
     const size_t copy_count = std::min(
         channel.count, required_samples - capture_count_);
     std::copy_n(
@@ -70,7 +90,7 @@ void FilteredSpectrumCollector::update() {
     decim_0_.configure(taps_audio_spectrum_halfband.taps);
     const buffer_c16_t capture{
         capture_.data(),
-        256 * capture_decimation_,
+        fft_samples * capture_decimation_,
         sampling_rate_ * capture_decimation_};
     const buffer_c16_t stage_0{
         stage_0_.data(),
@@ -88,3 +108,125 @@ void FilteredSpectrumCollector::update() {
     }
     capture_ready_ = false;
 }
+
+void AMFilteredSpectrumCollector::on_message(const Message* const message) {
+    if (message->id == Message::ID::UpdateSpectrum) {
+        update();
+    }
+    SpectrumCollector::on_message(message);
+}
+
+bool AMFilteredSpectrumCollector::start_capture(
+    const size_t decimation_factor) {
+    if (!valid_decimation(decimation_factor, maximum_decimation)) {
+        return false;
+    }
+
+    chSysLock();
+    if (capture_state_ != CaptureState::Idle) {
+        chSysUnlock();
+        return false;
+    }
+
+    capture_decimation_ = decimation_factor;
+    capture_count_ = 0;
+    capture_state_ = CaptureState::Capturing;
+    chSysUnlock();
+    return true;
+}
+
+bool AMFilteredSpectrumCollector::feed(
+    const buffer_c16_t& channel,
+    const int32_t filter_low_frequency,
+    const int32_t filter_high_frequency,
+    const int32_t filter_transition) {
+    chSysLock();
+    if (capture_state_ != CaptureState::Capturing) {
+        chSysUnlock();
+        return false;
+    }
+    chSysUnlock();
+
+    set_filter(
+        filter_low_frequency,
+        filter_high_frequency,
+        filter_transition);
+
+    const size_t required_samples = fft_samples * capture_decimation_;
+    const size_t copy_count = std::min(
+        channel.count, required_samples - capture_count_);
+    std::copy_n(
+        channel.p,
+        copy_count,
+        capture_.begin() + capture_count_);
+    capture_count_ += copy_count;
+
+    if (capture_count_ == required_samples) {
+        sampling_rate_ = channel.sampling_rate / capture_decimation_;
+        const bool streaming = is_streaming();
+
+        chSysLock();
+        if (streaming) {
+            capture_state_ = CaptureState::Pending;
+        } else {
+            capture_state_ = CaptureState::Idle;
+        }
+        chSysUnlock();
+
+        if (streaming) {
+            EventDispatcher::events_flag(EVT_MASK_SPECTRUM);
+        }
+        return true;
+    }
+    return false;
+}
+
+void AMFilteredSpectrumCollector::update() {
+    chSysLock();
+    if (capture_state_ != CaptureState::Pending) {
+        chSysUnlock();
+        return;
+    }
+    capture_state_ = CaptureState::Processing;
+    const size_t capture_decimation = capture_decimation_;
+    const uint32_t sampling_rate = sampling_rate_;
+    chSysUnlock();
+
+    size_t filtered_count = fft_samples * capture_decimation;
+    uint32_t filtered_sampling_rate = sampling_rate * capture_decimation;
+    size_t remaining_decimation = capture_decimation;
+
+    if (remaining_decimation == maximum_decimation) {
+        decimator_4_.configure(taps_audio_spectrum_decim_4.taps);
+        const buffer_c16_t input{
+            capture_.data(),
+            filtered_count,
+            filtered_sampling_rate};
+        const buffer_c16_t output{
+            capture_.data(),
+            filtered_count / decimator_4_.decimation_factor};
+        const auto filtered = decimator_4_.execute(input, output);
+        filtered_count = filtered.count;
+        filtered_sampling_rate = filtered.sampling_rate;
+        remaining_decimation /= decimator_4_.decimation_factor;
+    }
+
+    for (; remaining_decimation > 1; remaining_decimation /= 2) {
+        decimator_.configure(taps_audio_spectrum_halfband.taps);
+        const buffer_c16_t input{
+            capture_.data(),
+            filtered_count,
+            filtered_sampling_rate};
+        const buffer_c16_t output{
+            capture_.data(),
+            filtered_count / 2};
+        const auto filtered = decimator_.execute(input, output);
+        filtered_count = filtered.count;
+        filtered_sampling_rate = filtered.sampling_rate;
+    }
+
+    post_message({capture_.data(), filtered_count, filtered_sampling_rate});
+    chSysLock();
+    capture_state_ = CaptureState::Idle;
+    chSysUnlock();
+}

firmware/baseband/filtered_spectrum_collector.hpp

@@ -19,7 +19,7 @@ class FilteredSpectrumCollector : public SpectrumCollector {
    public:
     void on_message(const Message* const message);
 
-    void start_capture(const size_t decimation_factor);
+    bool start_capture(const size_t decimation_factor);
     bool feed(
         const buffer_c16_t& channel,
         const int32_t filter_low_frequency,
@@ -27,6 +27,9 @@ class FilteredSpectrumCollector : public SpectrumCollector {
         const int32_t filter_transition);
 
    private:
+    static constexpr size_t fft_samples = 256;
+    static constexpr size_t maximum_decimation = 4;
+
     std::array<complex16_t, 1024> capture_{};
     std::array<complex16_t, 512> stage_0_{};
     std::array<complex16_t, 256> stage_1_{};
@@ -40,4 +43,37 @@ class FilteredSpectrumCollector : public SpectrumCollector {
     void update();
 };
 
+class AMFilteredSpectrumCollector : public SpectrumCollector {
+   public:
+    void on_message(const Message* const message);
+
+    bool start_capture(const size_t decimation_factor);
+    bool feed(
+        const buffer_c16_t& channel,
+        const int32_t filter_low_frequency,
+        const int32_t filter_high_frequency,
+        const int32_t filter_transition);
+
+   private:
+    enum class CaptureState : uint8_t {
+        Idle,
+        Capturing,
+        Pending,
+        Processing,
+    };
+
+    static constexpr size_t fft_samples = 256;
+    static constexpr size_t maximum_decimation = 16;
+
+    std::array<complex16_t, fft_samples * maximum_decimation> capture_{};
+    dsp::decimate::FIRC16xR16x32Decim4 decimator_4_{};
+    dsp::decimate::FIRC16xR16x63HalfbandDecim2 decimator_{};
+    size_t capture_count_{0};
+    size_t capture_decimation_{1};
+    uint32_t sampling_rate_{0};
+    CaptureState capture_state_{CaptureState::Idle};
+
+    void update();
+};
+
 #endif /*__FILTERED_SPECTRUM_COLLECTOR_H__*/

firmware/baseband/proc_am_audio.cpp

@@ -51,8 +51,8 @@ void NarrowbandAMAudio::execute(const buffer_c8_t& buffer) {
     if (!spectrum_capture_active &&
         spectrum_samples >= spectrum_interval_samples) {
         spectrum_samples -= spectrum_interval_samples;
-        channel_spectrum.start_capture(spectrum_zoom_x2 ? 4 : 2);
-        spectrum_capture_active = true;
+        spectrum_capture_active =
+            channel_spectrum.start_capture(spectrum_decimation_factor);
     }
 
     if (spectrum_capture_active &&
@@ -146,7 +146,7 @@ void NarrowbandAMAudio::configure(const AMConfigureMessage& message) {
     channel_filter_transition = message.channel_filter.transition_normalized * channel_filter_input_fs;
 
     modulation_ssb = (int)message.modulation;  // now sending by message , 3 types of AM demod :   enum class Modulation : int32_t {DSB = 0, SSB = 1, SSB_FM = 2}
-    spectrum_zoom_x2 = message.channel_spectrum_decimation_factor == 2;
+    spectrum_decimation_factor = 2 * message.channel_spectrum_decimation_factor;
     channel_spectrum.set_decimation_factor(1);
     spectrum_interval_samples =
         decim_0_output_fs / spectrum_rate_hz;

firmware/baseband/proc_am_audio.hpp

@@ -70,7 +70,7 @@ class NarrowbandAMAudio : public BasebandProcessor {
     size_t spectrum_interval_samples{0};
     size_t spectrum_samples{0};
     bool spectrum_capture_active{false};
-    bool spectrum_zoom_x2{false};
+    size_t spectrum_decimation_factor{2};
 
     // bool modulation_ssb = false;  // Origianlly we only had 2 AM demod types {DSB = 0, SSB = 1} , and we could handle it with bool var , 1 bit.
     int8_t modulation_ssb = 0;  // Now we have 3 AM demod types we will send now index integer  {DSB = 0, SSB = 1, SSB_FM = 2}
@@ -80,7 +80,7 @@ class NarrowbandAMAudio : public BasebandProcessor {
     FeedForwardCompressor audio_compressor{};
     AudioOutput audio_output{};
 
-    FilteredSpectrumCollector channel_spectrum{};
+    AMFilteredSpectrumCollector channel_spectrum{};
 
     /* NB: Threads should be the last members in the class definition. */
 #ifdef PRALINE

firmware/baseband/proc_nfm_audio.cpp

@@ -57,8 +57,7 @@ void NarrowbandFMAudio::execute(const buffer_c8_t& buffer) {
     if (!spectrum_capture_active &&
         spectrum_samples >= spectrum_interval_samples) {
         spectrum_samples -= spectrum_interval_samples;
-        channel_spectrum.start_capture(2);
-        spectrum_capture_active = true;
+        spectrum_capture_active = channel_spectrum.start_capture(2);
     }
 
     if (spectrum_capture_active &&