ptp_filters.h

Go to the documentation of this file.

// Copyright 2024 The Aerospace Corporation.
// This file is a part of SatCat5, licensed under CERN-OHL-W v2 or later.
 
#pragma once
 
#include <satcat5/list.h>
#include <satcat5/ptp_time.h>
#include <satcat5/wide_integer.h>
 
namespace satcat5 {
    namespace ptp {
        class Filter {
        public:
            virtual void reset() = 0;
 
            virtual void rate(s64 delta_subns, u32 elapsed_usec) {}
 
            virtual s64 update(s64 next, u32 elapsed_usec) = 0;
 
        protected:
            constexpr Filter() : m_next(0) {}
            ~Filter() {}
 
        private:
            // Linked-list of chained filter objects.
            friend satcat5::util::ListCore;
            satcat5::ptp::Filter* m_next;
        };
 
        class DebugFilter final : public satcat5::ptp::Filter {
        public:
            constexpr DebugFilter()
                : m_prev(0), m_usec(0) {}
 
            void reset() override
                { m_prev = 0; m_usec = 0; }
 
            s64 update(s64 next, u32 elapsed_usec) override
                { m_prev = next; m_usec = elapsed_usec; return next; }
 
            inline s64 prev() const { return m_prev; }
            inline u32 usec() const { return m_usec; }
 
        protected:
            s64 m_prev;
            u32 m_usec;
        };
 
        template<typename T, unsigned MAX_WINDOW>
        class SlidingWindow final {
        public:
            SlidingWindow() : m_wridx(0), m_window{0} {}
            ~SlidingWindow() {}
 
            // Copy the N most recent samples from the working buffer.
            // (The last index of the output array is the most recent sample.)
            void read(T* dst, unsigned count) const {
                if (m_wridx >= count) {
                    memcpy(dst, m_window + m_wridx - count, count*sizeof(T));
                } else {
                    unsigned wrap = count - m_wridx;
                    memcpy(dst, m_window + MAX_WINDOW - wrap, wrap*sizeof(T));
                    memcpy(dst + wrap, m_window, m_wridx*sizeof(T));
                }
            }
 
            // Write one new sample to the working buffer.
            void push(const T& next) {
                m_window[m_wridx] = next;
                if (++m_wridx >= MAX_WINDOW) m_wridx = 0;
            }
 
            void reset() {
                memset(m_window, 0, MAX_WINDOW * sizeof(T));
            }
 
        protected:
            unsigned m_wridx;
            T m_window[MAX_WINDOW];
        };
 
        // Low-level functions implemented outside the template classes.
        s64 boxcar_filter(const s64* data, unsigned order);
        s64 median_filter(s64* data, unsigned samps);
 
        class AmplitudeReject : public satcat5::ptp::Filter {
        public:
            explicit AmplitudeReject(unsigned tau_msec = 10000);
 
            // Accessors for settings and internal state.
            inline s64 get_mean() const         {return m_mean;}
            inline u64 get_sigma() const        {return m_sigma;}
            inline void set_min(u64 min_subns)  {m_min = min_subns;}
            inline void set_tau(u32 tau_msec)   {m_tau_usec = 1000*tau_msec;}
 
            // Required API from ptp::Filter.
            void reset() override;
            s64 update(s64 next, u32 elapsed_usec) override;
 
        protected:
            s64 m_mean;
            u64 m_sigma;
            u64 m_min;
            u32 m_tau_usec;
        };
 
        template<unsigned MAX_ORDER>
        class BoxcarFilter : public satcat5::ptp::Filter {
        public:
            explicit BoxcarFilter(unsigned order = MAX_ORDER)
                : m_order(0) {set_order(order);}
 
            inline void set_order(unsigned x)
                { if (x <= MAX_ORDER) m_order = x; }
 
            void reset() override
                { m_window.reset(); }
 
            s64 update(s64 next, u32 elapsed_usec) override {
                if (next == INT64_MAX) return INT64_MAX;
                s64 temp[MAX_WINDOW];
                m_window.push(next);
                m_window.read(temp, 1u << m_order);
                return satcat5::ptp::boxcar_filter(temp, m_order);
            }
 
        protected:
            static constexpr unsigned MAX_WINDOW = 1u << MAX_ORDER;
            unsigned m_order;
            satcat5::ptp::SlidingWindow<s64, MAX_WINDOW> m_window;
        };
 
        template<unsigned MAX_ORDER>
        class MedianFilter : public satcat5::ptp::Filter {
        public:
            explicit MedianFilter(unsigned order = MAX_ORDER)
                : m_order(0) {set_order(order);}
 
            inline void set_order(unsigned x)
                { if (x <= MAX_ORDER) m_order = x|1; }
 
            void reset() override
                { m_window.reset(); }
 
            s64 update(s64 next, u32 elapsed_usec) override {
                if (next == INT64_MAX) return INT64_MAX;
                s64 temp[MAX_ORDER];
                m_window.push(next);
                m_window.read(temp, m_order);
                return satcat5::ptp::median_filter(temp, m_order);
            }
 
        protected:
            unsigned m_order;
            satcat5::ptp::SlidingWindow<s64, MAX_ORDER|1> m_window;
        };
 
        struct CoeffPI {
        public:
            explicit constexpr CoeffPI(double tau_secs)
                : kp(satcat5::util::round_u64z(k1(tau_secs, 0.707) / fw_gain()))
                , ki(satcat5::util::round_u64z(k2(tau_secs, 0.707) / fw_gain()))
                {} // No other initialization required.
            CoeffPI(const CoeffPI& t) = default;
            CoeffPI& operator=(const CoeffPI& t) = default;
 
            bool ok() const {return (kp > 7) && (ki > 7);}
 
            static constexpr unsigned SCALE = 60;
 
        protected:
            // Calculate alpha2, K1, and K2 from Stephens & Thomas Table II.
            // Note: Omit scaling by T0; compensate for this at runtime.
            static constexpr double alpha(double zeta)
                { return 0.25 / (zeta * zeta); }
            static constexpr double k1(double tau, double zeta)
                { return  1.273239545 / (tau * (1.0 + alpha(zeta))); }
            static constexpr double k2(double tau, double zeta)
                { return alpha(zeta) * k1(tau, zeta) * k1(tau, zeta); }
            // End-to-end loop gain including intermediate scaling:
            //  * T0 compensation: Multiply by assumed T0 = 1 sec.
            //  * Cycles to radians: Effective gain = 1 / (2*pi).
            //  * Output scaling: Divide final output by 2^SCALE.
            static constexpr double fw_gain()
                { return double(satcat5::ptp::USEC_PER_SEC)
                       / 6.28318530717958647693
                       / satcat5::util::pow2d(SCALE); }
 
            friend satcat5::ptp::ControllerPI;
            u64 kp;     // Proportional coefficient (LSB per subns)
            u64 ki;     // Integral coefficient (LSB per subns)
        };
 
        class ControllerPI : public satcat5::ptp::Filter {
        public:
            explicit ControllerPI(const satcat5::ptp::CoeffPI& coeff);
 
            void set_coeff(const satcat5::ptp::CoeffPI& coeff);
 
            inline void set_slew(u64 slew) { m_slew = slew; }
 
            void reset() override;
            void rate(s64 delta, u32 elapsed_usec) override;
            s64 update(s64 next, u32 elapsed_usec) override;
 
        protected:
            // Internal state.
            satcat5::ptp::CoeffPI m_coeff;
            satcat5::util::int128_t m_accum;
            u64 m_slew;
        };
 
        struct CoeffPII {
        public:
            explicit constexpr CoeffPII(double tau_secs)
                : kp(satcat5::util::round_u64z(k1(tau_secs) / fw_gain()))
                , ki(satcat5::util::round_u64z(k2(tau_secs) / fw_gain()))
                , kr(satcat5::util::round_u64z(kratio(tau_secs)))
                {} // No other initialization required.
            CoeffPII(const CoeffPII& t) = default;
            CoeffPII& operator=(const CoeffPII& t) = default;
 
            bool ok() const {return (kp > 7) && (ki > 7) && (kr > 7);}
 
            static constexpr unsigned SCALE1 = 70;
            static constexpr unsigned SCALE2 = 64;
            static constexpr unsigned SCALE = SCALE1 + SCALE2;
 
        protected:
            // "Standard underdamped" K1, K2, and K2 from Stephens & Thomas Table III.
            // Note: Omit scaling by T0; compensate for this at runtime.
            static constexpr double k1(double tau)
                { return 0.830373616 / tau; }   // i.e., 60 / 23pi
            static constexpr double k2(double tau)
                { return (4.0/9.0) * k1(tau) * k1(tau); }
            static constexpr double k3(double tau)
                { return (2.0/27.0) * k1(tau) * k1(tau) * k1(tau); }
            // Ratio of K3 / K2, used for nested-accumulator updates.
            static constexpr double kratio(double tau)
                { return k3(tau) / k2(tau)
                       * satcat5::util::pow2d(SCALE2)
                       / double(satcat5::ptp::USEC_PER_SEC); }
            // End-to-end loop gain for including intermediate scaling:
            //  * T0 compensation: Multiply by assumed T0 = 1 sec.
            //  * Cycles to radians: Effective gain = 1 / (2*pi).
            //  * Output scaling: Divide final output by 2^SCALE1.
            static constexpr double fw_gain()
                { return double(satcat5::ptp::USEC_PER_SEC)
                       / 6.28318530717958647693
                       / satcat5::util::pow2d(SCALE1); }
 
            friend satcat5::ptp::ControllerPII;
            u64 kp;     // Proportional coefficient (LSB per subns)
            u64 ki;     // Integral coefficient (LSB per subns)
            u64 kr;     // Double-integral coefficient (K3 / K2)
        };
 
        class ControllerPII : public satcat5::ptp::Filter {
        public:
            explicit ControllerPII(const satcat5::ptp::CoeffPII& coeff);
 
            void set_coeff(const satcat5::ptp::CoeffPII& coeff);
 
            inline void set_slew(u64 slew) { m_slew = slew; }
 
            void reset() override;
            void rate(s64 delta, u32 elapsed_usec) override;
            s64 update(s64 next, u32 elapsed_usec) override;
 
        protected:
            // Internal state.
            satcat5::ptp::CoeffPII m_coeff;
            satcat5::util::int128_t m_accum1;
            satcat5::util::int256_t m_accum2;
            u64 m_slew;
        };
 
        struct LinearRegression {
            static constexpr unsigned TSCALE = 48;
            satcat5::util::int128_t alpha;  // Intercept at x = 0
            satcat5::util::int128_t beta;   // Slope * 2^TSCALE
 
            constexpr LinearRegression()
                : alpha(satcat5::util::INT128_ZERO)
                , beta(satcat5::util::INT128_ZERO) {}
            LinearRegression(const LinearRegression& t) = default;
            LinearRegression& operator=(const LinearRegression& t) = default;
 
            LinearRegression(const unsigned n, const s64* x, const s64* y);
 
            s64 extrapolate(s64 x) const;
        };
 
        struct CoeffLR {
        public:
            explicit constexpr CoeffLR(double tau_secs)
                : ki(satcat5::util::round_u64z(ki_gain() / tau_secs))
                , kw(satcat5::util::round_u64z(kw_gain() * 2.0 / tau_secs))
                {} // No other initialization required.
            CoeffLR(const CoeffLR& t) = default;
            CoeffLR& operator=(const CoeffLR& t) = default;
 
            bool ok() const {return (ki > 7) && (kw > 7);}
 
        protected:
            static constexpr double ki_gain()
                { return double(satcat5::ptp::USEC_PER_SEC); }
            static constexpr double kw_gain()
                { return satcat5::util::pow2d(LinearRegression::TSCALE)
                       / double(satcat5::ptp::USEC_PER_SEC); }
 
            friend class satcat5::ptp::ControllerLR_Inner;
            u64 ki;     // Integral coefficient (LSB per subns)
            u64 kw;     // Intercept scaling factor (LSB per usec)
        };
 
        class ControllerLR_Inner : public satcat5::ptp::Filter {
        public:
            void set_coeff(const satcat5::ptp::CoeffLR& coeff);
 
            void rate(s64 delta, u32 elapsed_usec) override;
 
        protected:
            ControllerLR_Inner(const satcat5::ptp::CoeffLR& coeff, unsigned window);
            ~ControllerLR_Inner() {}
 
            // Signal processing functions.
            s64 update_inner(const u32* dt, const s64* y);
 
            // Internal state, not including sliding-window buffers.
            satcat5::ptp::CoeffLR m_coeff;
            satcat5::util::int128_t m_accum;
            unsigned m_window;
        };
 
        template<unsigned MAX_WINDOW>
        class ControllerLR : public satcat5::ptp::ControllerLR_Inner {
        public:
            explicit ControllerLR(const satcat5::ptp::CoeffLR& coeff)
                : satcat5::ptp::ControllerLR_Inner(coeff, MAX_WINDOW)
                , m_count(0), m_elapsed(0) {}
            static_assert(MAX_WINDOW >= 2, "MAX_WINDOW must be at least 2.");
 
            void set_window(unsigned window) {
                if (2 <= window && window <= MAX_WINDOW) m_window = window;
            }
 
            void reset() override {
                m_dly.reset();
                m_dat.reset();
                m_accum = satcat5::util::INT128_ZERO;
            }
 
            s64 update(s64 next, u32 elapsed_usec) override {
                // Push valid samples into the sliding-window buffers.
                // (Elapsed time still increments even if we drop a sample.)
                m_elapsed += elapsed_usec;
                if (next == INT64_MAX) return INT64_MAX;
                m_dly.push(m_elapsed);
                m_dat.push(next);
                m_elapsed = 0;
                // Attempt to read a full window of samples...
                if (m_count < MAX_WINDOW) ++m_count;
                if (m_count < m_window) return INT64_MAX;
                u32 temp_dly[MAX_WINDOW];   m_dly.read(temp_dly, m_window);
                s64 temp_dat[MAX_WINDOW];   m_dat.read(temp_dat, m_window);
                // Proceed with linear-regression processing.
                return update_inner(temp_dly, temp_dat);
            }
 
        protected:
            unsigned m_count;
            u32 m_elapsed;
            satcat5::ptp::SlidingWindow<u32, MAX_WINDOW> m_dly;
            satcat5::ptp::SlidingWindow<s64, MAX_WINDOW> m_dat;
        };
 
        class LinearPrediction : public satcat5::ptp::Filter {
        public:
            explicit constexpr LinearPrediction(satcat5::ptp::Filter* ctrl = 0)
                : m_filters(ctrl), m_first(true), m_rate(0)
                , m_accum(satcat5::util::INT128_ZERO) {}
 
            inline void add_filter(satcat5::ptp::Filter* filter)
                { m_filters.push_back(filter); }
 
            void reset() override;
            void rate(s64 delta, u32 elapsed_usec) override;
            s64 update(s64 next, u32 elapsed_usec) override;
 
            s64 predict(u32 elapsed_usec) const;
 
        protected:
            // Convert normalized rate to match accumulator scale.
            static constexpr unsigned SCALE = 32;
            satcat5::util::int128_t incr(u32 elapsed_usec) const;
 
            // Internal state.
            satcat5::util::List<satcat5::ptp::Filter> m_filters;
            bool m_first;
            s64 m_rate;
            satcat5::util::int128_t m_accum;
        };
 
        class RateConversion {
        public:
            constexpr RateConversion(double ref_clk_hz, unsigned scale_ns)
                : m_scale(satcat5::util::round_s64z(fw_gain(scale_ns) / ref_clk_hz)) {}
 
            bool ok() const {return satcat5::util::abs_s64(m_scale) > 1000000;}
 
            s64 convert(s64 offset) const;
 
            s64 invert(s64 rate) const;
 
        protected:
            // Internal scaling is optimized for 1-200 MHz clocks.
            static constexpr unsigned SHIFT = 48;
            static constexpr double fw_gain(unsigned scale_ns) {
                return satcat5::util::pow2d(scale_ns + SHIFT)
                     / double(satcat5::ptp::SUBNS_PER_NSEC);
            }
            s64 m_scale;
        };
    }
}