add: 添加了文件

XiangYyang · XiangYyang · commit 8202d5c3fe2b · 2023-04-25T01:23:56.000+08:00
diff --git a/.gitignore b/.gitignore
@@ -0,0 +1,27 @@
+
+.vscode
+.pyenv
+
+target
+build
+target
+Core
+Drivers
+FATFS
+Middlewares
+USB_Device
+HTML
+
+!*.c
+!*.h
+!*.cpp
+!*.hpp
+
+!Makefile
+
+!*.a
+
+!*.s
+!*.ld
+
+!Doxyfile
diff --git a/COPYING b/COPYING
diff --git a/README.md b/README.md
@@ -0,0 +1,6 @@
+SOGI-PLL
+========
+
+Phase-Locked Loop Based Second Order Generalized Integrator for power electric application.
+
+
diff --git a/inc/mc_config.h b/inc/mc_config.h
@@ -0,0 +1,50 @@
+// SPDX-License-Identifier: LGPL-2.1
+
+/**
+ * @file    mc_config.h
+ * @brief   控制系统配置
+ * @author  向阳 (hinata.hoshino@foxmail.com)
+ * @date    2022-07-27
+ * 
+ * @copyright Copyright (C) 向阳 2022
+ */
+#if !defined(__INCLUDE_CONTROL_MC_CONFIG_H__)
+#define __INCLUDE_CONTROL_MC_CONFIG_H__
+#include <float.h>
+namespace control
+{
+    //! 运算的值类型
+    using value_t = float;
+
+    //! 控制周期
+    constexpr value_t T = 1 / 12800.0f;
+
+    //! 常量值表
+    template<typename T>
+    struct constant_value;
+
+    
+    template<>
+    struct constant_value<value_t>
+    {
+        //! 零
+        static constexpr value_t ZERO = 0;
+
+        //! 圆周率
+        static constexpr value_t PI = 3.14159265358979323f;
+
+        //! 1/2 * 圆周率
+        static constexpr value_t HALF_PI = PI / 2;
+    
+        //! 2 * 圆周率
+        static constexpr value_t TAU = PI * 2;
+
+        //! 最大值
+        static constexpr value_t MAX = FLT_MAX;
+    
+        //! 最小
+        static constexpr value_t MIN = FLT_MIN;
+    };
+
+}
+#endif // __INCLUDE_CONTROL_MC_SPLL_H__
diff --git a/inc/mc_pid.h b/inc/mc_pid.h
@@ -0,0 +1,119 @@
+// SPDX-License-Identifier: LGPL-2.1
+
+/**
+ * @file    ma_pid.h
+ * @brief   数字PID控制器
+ * @author  向阳 (hinata.hoshino@foxmail.com)
+ * @date    2022-07-16
+ *
+ * @copyright Copyright (C) 向阳 2022
+ */
+#if !defined(__INCLUDE_CONTROL_MC_PID_H__)
+#define __INCLUDE_CONTROL_MC_PID_H__
+#include "mc_config.h"
+namespace control
+{
+    /**
+     * @brief 带退饱和反馈和积分限幅的PID控制器(float32)
+     *
+     */
+    class PID final
+    {
+    public:
+        /**
+         * @brief 控制器参数
+         *
+         */
+        struct Factors
+        {
+            //! 比例系数
+            value_t kp;
+
+            //! 积分系数
+            value_t ki;
+
+            //! 微分系数
+            value_t kd;
+
+            //! 微分增益
+            value_t kg;
+
+            //! 饱和系数
+            value_t kc;
+
+            //! 积分器限幅(min)
+            value_t i_min;
+
+            //! 积分器限幅(max)
+            value_t i_max;
+        };
+
+        PID() = default;
+        ~PID() = default;
+
+        PID(const PID &) = delete;
+        PID &operator=(const PID &) = delete;
+
+        PID(PID &&) = delete;
+        PID &operator=(PID &&) = delete;
+
+        //! @get
+        //! @set
+        //! 控制器参数
+        Factors param = {0};
+
+        /**
+         * @brief 重置调节器状态
+         *
+         */
+        void reset() noexcept;
+
+        /**
+         * @brief 数字P
+         *
+         * @param [in] e: 输入, error
+         *
+         * @return value_t: 输出
+         *
+         */
+        value_t p_transfer(value_t e) noexcept;
+
+        /**
+         * @brief 数字PI
+         *
+         * @param [in] e: 输入, error
+         *
+         * @return value_t: 输出
+         *
+         */
+        value_t pi_transfer(value_t e) noexcept;
+
+        /**
+         * @brief 数字PD
+         *
+         * @param [in] e: 输入, error
+         *
+         * @return value_t: 输出
+         *
+         */
+        value_t pd_transfer(value_t e) noexcept;
+
+        /**
+         * @brief 数字PID
+         *
+         * @param [in] e: 输入, error
+         *
+         * @return value_t: 输出
+         *
+         */
+        value_t pid_transfer(value_t e) noexcept;
+
+    private:
+        //! 饱和误差
+        value_t sat_err = 0;
+
+        //! 积分值
+        value_t i_sum = 0;
+    };
+}
+#endif // __INCLUDE_CONTROL_MC_PID_H__
diff --git a/inc/mc_spll.h b/inc/mc_spll.h
@@ -0,0 +1,122 @@
+// SPDX-License-Identifier: LGPL-2.1
+
+/**
+ * @file    mc_spll.h
+ * @brief   数字锁相环
+ * @author  向阳 (hinata.hoshino@foxmail.com)
+ * @date    2022-07-27
+ * 
+ * @copyright Copyright (C) 向阳 2022
+ */
+#if !defined(__INCLUDE_CONTROL_MC_SPLL_H__)
+#define __INCLUDE_CONTROL_MC_SPLL_H__
+#include <tuple>
+#include "mc_pid.h"
+#include <stddef.h>
+#include <stdint.h>
+#include "mc_config.h"
+namespace control
+{
+    class SPLL final
+    {
+    public:
+        //! SPLL工作周期
+        static constexpr value_t Ti = T;
+
+        //! SPLL期望频率(Hz)
+        static constexpr value_t TARGET_FREQ = 50;
+
+        //! 计算1个周期有多少个点
+        static constexpr size_t N_SAMPLE = static_cast<size_t>((1 / Ti) / TARGET_FREQ);
+
+        SPLL();
+        ~SPLL() = default;
+
+        SPLL(SPLL&&) = delete;
+        SPLL& operator=(SPLL&&) = delete;
+
+        SPLL(const SPLL &) = delete;
+        SPLL& operator=(const SPLL &) = delete;
+
+        /**
+         * @brief 复位
+         * 
+         */
+        void reset();
+
+        /**
+         * @brief 进行一次计算
+         * 
+         * @note  按照Ti等间隔调用本方法, 实现PLL
+         * 
+         */
+        void transfer_1phase(value_t val);
+
+        /**
+         * @brief 判断PLL是否已经锁定
+         *
+         * @param [in] th: 比较阈值
+         * 
+         */
+        bool is_lock(value_t th = 1e-2f) const;
+
+        /**
+         * @brief 取得当前信号的频率
+         *
+         * @return value_t: 频率
+         * 
+         */
+        inline value_t freq() const noexcept
+        {
+            return omega / constant_value<value_t>::TAU;
+        }
+
+        /**
+         * @brief 取得当前信号的相位
+         *
+         * @return value_t: 相位
+         * 
+         */
+        inline value_t phase() const noexcept
+        {
+            return cur_phase;
+        }
+    private:
+        //! PI
+        PID pid;
+
+        //! 是否启动环路
+        bool launch_loop;
+
+        //! 采样点索引, 最大到N_SAMPLE / 4
+        uint16_t sample_index;
+
+        //! 角速度
+        value_t omega;
+
+        //! 角度积分值, 作为相位wt
+        value_t cur_phase;
+
+        //! 自动归零: 最大值和最小值
+        value_t auto_offset_min;
+        value_t auto_offset_max;
+
+        //! SOGI的积分值
+        value_t sogi_s1;
+        value_t sogi_s2;
+
+
+        //! 上次的环路error
+        value_t last_error;
+    
+        /**
+         * @brief 按照历史最大值和最小值进行归中
+         * @param [in] inp: 输入
+         *
+         * @return value_t: 归中后的值
+         * 
+         */
+        value_t auto_offset(value_t inp);
+    };
+}
+#endif // __INCLUDE_CONTROL_MC_SPLL_H__
diff --git a/src/mc_pid.cpp b/src/mc_pid.cpp
@@ -0,0 +1,65 @@
+// SPDX-License-Identifier: LGPL-2.1
+
+/**
+ * @file    mc_pid.cpp
+ * @brief   数字PID控制器
+ * @author  向阳 (hinata.hoshino@foxmail.com)
+ * @date    2022-07-16
+ * 
+ * @copyright Copyright (C) 向阳 2022
+ */
+#include "mc_pid.h"
+
+using namespace control;
+
+void PID::reset() noexcept
+{
+    i_sum = 0;
+    sat_err = 0;
+}
+
+value_t PID::p_transfer(value_t e) noexcept
+{
+    value_t m = param.kp * e;
+    value_t out
+        = m > param.i_max ? param.i_max
+        : m < param.i_min ? param.i_min
+        : m;
+    return out;
+}
+
+value_t PID::pi_transfer(value_t e) noexcept
+{
+    value_t sat = param.kp * e + i_sum;
+    // PI输出
+    // U(s) = kp * E(s) + ki * E(s) / s
+    value_t out
+        = sat > param.i_max ? param.i_max
+        : sat < param.i_min ? param.i_min
+        : sat;
+    // 饱和误差
+    value_t sat_err = out - sat;
+    // 累积积分, 加上饱和误差
+    i_sum += param.ki * e + param.kc * sat_err;
+    // 积分限幅, clamp
+    if (i_sum > param.i_max)
+    {
+        i_sum = param.i_max;
+    }
+    else if (i_sum < param.i_min)
+    {
+        i_sum = param.i_min;
+    }
+    //
+    return out;
+}
+
+value_t PID::pd_transfer(value_t e) noexcept
+{
+    return 0;
+}
+
+value_t PID::pid_transfer(value_t e) noexcept
+{
+    return 0;
+}
diff --git a/src/mc_spll.cpp b/src/mc_spll.cpp
