chore: update

ads/include/control/control_params.h

@@ -1,3 +1,29 @@
 #pragma once
 
-
+struct LatControlParams
+{
+    /// \brief Corner stiffness; front
+    double cf;
+    /// \brief Corner stiffness; rear
+    double cr;
+    /// \brief Distance between front and rear wheel center
+    double wheelbase;
+    /// \brief Mass of the vehicle
+    double mass;
+    /// \brief Distance from front wheel center to COM
+    double lf;
+    /// \brief Distance from rear wheel center to COM
+    double lr;
+    /// \brief Rotational inertia
+    double iz;
+    /// \brief Matrix_k_(0,2) weight parameters
+    double weight_param;
+    /// \brief
+    double y_weight_param;
+    /// \brief
+    double heading_weight_param;
+    /// \brief
+    double forward_param;
+    /// \brief
+    double weight_param_lat;
+};

ads/include/math/digital_filter.h

@@ -0,0 +1,138 @@
+#pragma once
+
+#include <cassert> /**< assert */
+#include <cmath> /**< std::abs M_PI std::sqrt */
+#include <deque>
+#include <vector>
+
+class DigitalFilter
+{
+
+    const double kDoubleEpsilon = 1.0e-6;
+
+public:
+    DigitalFilter() = default;
+    ~DigitalFilter() = default;
+
+    inline double Filter(const double x) {
+        if (denominators_.empty() || numerators_.empty()) {
+            return 0.0;
+        }
+
+        xs_.pop_back();
+        xs_.push_front(x);
+        const double xside =
+                Compute(xs_, numerators_, 0, numerators_.size() - 1);
+
+        ys_.pop_back();
+        const double yside =
+                Compute(ys_, denominators_, 1, denominators_.size() - 1);
+
+        double y = 0.0;
+        if (std::abs(denominators_.front()) > kDoubleEpsilon) {
+            y = (xside - yside) / denominators_.front();
+        }
+        ys_.push_front(y);
+
+        return UpdateLast(ys_);
+    }
+
+    inline void SetDenominators(const std::vector<double> &denominators) {
+        denominators_.assign(denominators.begin(), denominators.end());
+        ys_.resize(denominators_.size(), 0.0);
+    }
+
+    inline void SetNumerators(const std::vector<double> &numerators) {
+        numerators_.assign(numerators.begin(), numerators.end());
+        xs_.resize(numerators_.size(), 0.0);
+    }
+
+    inline void SetCoefficients(const double cutoff_freq, const double ts, std::vector<double> &denominators,
+                          std::vector<double> &numerators) {
+        LpfCoefficients(ts, cutoff_freq, denominators, numerators);
+        SetDenominators(denominators);
+        SetNumerators(numerators);
+    }
+
+    void SetDeadZone(const double deadzone) {
+        deadzone_ = std::abs(deadzone);
+    }
+
+    inline const std::vector<double> &Denominators() const {
+        return denominators_;
+    }
+
+    inline const std::vector<double> &Numerators() const {
+        return numerators_;
+    }
+
+    inline double DeadZone() const {
+        return dead_zone_;
+    }
+
+private:
+    inline double UpdateLast(const double input) {
+        const double diff = std::abs(input - last_);
+
+        if (diff < deadzone_)
+        {
+            return last_;
+        }
+
+        last = input;
+        return input;
+    }
+
+    inline double Compute(const std::deque<double> &values,
+                   const std::vector<double> &coefficients,
+                   const std::size_t coeff_start, const std::size_t coeff_end) {
+        assert(coeff_start <= coeff_end && coeff_end < coefficients.size());
+        assert((coeff_end - coeff_start + 1) == values.size());
+
+        double sum = 0.0;
+        auto i = coeff_start;
+
+        for (const auto value : values)
+        {
+            sum += value * coefficients[i];
+            ++i;
+        }
+
+        return sum;
+    }
+
+    inline void LpfCoefficients(const double ts, const double cutoff_freq,
+                         std::vector<double> &denominators,
+                         std::vector<double> &numerators) {
+        denominators.clear();
+        numerators.clear();
+
+        denominators.reserve(3);
+        numerators.reserve(3);
+
+        double wa = 2.0 * M_PI * cutoff_freq;  // Analog frequency in rad/s
+        double alpha = wa * ts / 2.0;          // tan(Wd/2), Wd is discrete frequency
+        double alpha_sqr = alpha * alpha;
+        double tmp_term = std::sqrt(2.0) * alpha + alpha_sqr;
+        double gain = alpha_sqr / (1.0 + tmp_term);
+
+        denominators.push_back(1.0);
+        denominators.push_back(2.0 * (alpha_sqr - 1.0) / (1.0 + tmp_term));
+        denominators.push_back((1.0 - std::sqrt(2.0) * alpha + alpha_sqr) /
+                                (1.0 + tmp_term));
+
+        numerators.push_back(gain);
+        numerators.push_back(2.0 * gain);
+        numerators.push_back(gain);
+    }
+
+    std::deque<double> xs_;
+    std::deque<double> ys_;
+    std::vector<double> denominators_;
+    std::vector<double> numerators_;
+    double deadzone_ = 0.0;
+    double last_ = 0.0;
+};
+
+
+

ads/include/math/fir_filter.h

@@ -0,0 +1,84 @@
+#pragma once
+
+#include <cstdint>
+#include <cmath>
+#include <vector>
+
+// FIR filter: Finite Impulse Response filter
+class FirFilter
+{
+public:
+    bool InitializaAsLowPass(const float sample_rate,
+                             const uint32_t frame_length,
+                             const float cutoff_freq,
+                             const float trans_bw);
+
+    bool Configure(const float sample_rate,
+                   const uint32_t frame_length,
+                   const float cutoff_freq,
+                   const float low_trans_bw,
+                   const float high_cutoff_freq,
+                   const float high_trans_bw);
+    void Reset();
+
+    void ApplyFilter(const std::vector<float> &input_frame_samples,
+                     std::vector<float> &output_frame_samples);
+
+    bool PastInitialTransient();
+
+    void Test(const float frequency,
+              const float duration);
+private:
+        void setFilterCoefficientsAndLength(const float sampleRate,
+                                        const float lowCutoff,
+                                        const float lowTransBw,
+                                        const float highCutoff,
+                                        const float highTransBw);
+
+    void calcLowPassFilterCoefficients(const float fhaHighDivFs,
+                                       float coefficients[],
+                                       const uint32_t length);
+
+
+
+    void applyDelayLineFilter(const float inputFrameSamples[],
+                              float outputFrameSamples[]);
+
+    void applyInputSideConvolutionFilter(const float inputFrameSamples[],
+                                         float outputFrameSamples[]);
+
+    void applyOutSideconvolutionFilter(const float inputFrameSamples[],
+                                       float outputFrameSamples[]);
+
+    void applyFFTconvolutionFilter(const float inputFrameSamples[],
+                                   float outputFrameSamples[]);
+
+    float sampleRate;
+    float lowCutoffFreq;
+    float lowTransBw;
+    float highCutoffFreq;
+    float highTransBw;
+
+    uint32_t frameLength;
+    uint32_t filterLength;
+    uint32_t fftLength;
+    uint32_t fftLengthDiv2;
+    uint32_t convolutionResultLength;
+    uint32_t overlapLength;
+
+    float* filterCoefficients;
+    float* filterPadded;
+    float* signalPadded;
+    float* signalPaddedXfilterPadded;
+    float* overlapBuffer;
+
+    float* tempResult;
+    float* tempResultB;
+
+    float filterNyquistValue;
+    FFT* fft;
+
+    uint32_t initialTransientSampleCount;
+    uint32_t prevInitialTransientSampleCount;
+};
+