├── LICENSE ├── README.md ├── hpf_1st.c ├── hpf_1st.h ├── lpf_1st.c ├── lpf_1st.h ├── maf.c ├── maf.h ├── notch_filter.c ├── notch_filter.h ├── pid.c ├── pid.h ├── qfoc.c └── qfoc.h /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 luoqi 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # QFOC 2 | 3 | **A very simple to use foc lib, used to control a PMSM motor**. 4 | 5 | This lib only containe the core of foc algrithm, users need to 6 | use the appling api in thier own mcu or other platform. 7 | 8 | -------------------------------------------------------------------------------- /hpf_1st.c: -------------------------------------------------------------------------------- 1 | /* 2 | * @Author: luoqi 3 | * @Date: 2022-11-29 09:44:24 4 | * @Last Modified by: luoqi 5 | * @Last Modified time: 2022-11-29 09:53:35 6 | */ 7 | 8 | #include "hpf_1st.h" 9 | 10 | #ifndef M_PI 11 | #define M_PI 3.14159265358979323846 12 | #endif 13 | 14 | int hpf_1st_init(Hpf1stObj *filter, qfp_t fc, qfp_t fs) 15 | { 16 | if(!filter || fs <= 0) { 17 | return -1; 18 | } 19 | qfp_t ts = 1 / fs; 20 | filter->alpha = 1 / (1 + 2 * M_PI * fc * ts); 21 | filter->ts = ts; 22 | filter->fc = fc; 23 | filter->u_k1 = 0; 24 | filter->y_k1 = 0; 25 | return 0; 26 | } 27 | 28 | qfp_t hpf_1st_calcu(Hpf1stObj *filter, qfp_t u_k) 29 | { 30 | if(!filter) { 31 | return 0; 32 | } 33 | qfp_t y_k = filter->alpha * (u_k - filter->u_k1 + filter->y_k1); 34 | filter->y_k1 = y_k; 35 | filter->u_k1 = u_k; 36 | return y_k; 37 | } 38 | 39 | int hpf_1st_fc_set(Hpf1stObj *filter, qfp_t fc) 40 | { 41 | if(!filter || fc <= 0) { 42 | return -1; 43 | } 44 | filter->fc = fc; 45 | filter->alpha = 1 / (1 + 2 * M_PI * fc * filter->ts); 46 | return 0; 47 | } 48 | -------------------------------------------------------------------------------- /hpf_1st.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @Author: luoqi 3 | * @Date: 2022-03-16 09:26:15 4 | * @Last Modified by: luoqi 5 | * @Last Modified time: 2022-11-29 09:53:46 6 | */ 7 | 8 | #ifndef _HPF_1ST_H 9 | #define _HPF_1ST_H 10 | 11 | #ifdef __cplusplus 12 | extern "C" { 13 | #endif 14 | 15 | #ifndef qfp_t 16 | typedef float qfp_t; 17 | #endif 18 | 19 | typedef struct _hpf_first_order 20 | { 21 | qfp_t fc; // cut-off frequency 22 | qfp_t y_k1; // last output 23 | qfp_t alpha; // filter coefficient 24 | qfp_t ts; // samping period 25 | qfp_t u_k1; // last input 26 | }Hpf1stObj; 27 | 28 | int hpf_1st_init(Hpf1stObj *filter, qfp_t fc, qfp_t fs); 29 | 30 | qfp_t hpf_1st_calcu(Hpf1stObj *filter, qfp_t u_k); 31 | 32 | int hpf_1st_fc_set(Hpf1stObj *filter, qfp_t fc); 33 | 34 | #ifdef __cplusplus 35 | } 36 | #endif 37 | 38 | #endif -------------------------------------------------------------------------------- /lpf_1st.c: -------------------------------------------------------------------------------- 1 | /* 2 | * @Author: luoqi 3 | * @Date: 2022-03-16 09:27:04 4 | * @Last Modified by: luoqi 5 | * @Last Modified time: 2022-11-29 09:54:57 6 | */ 7 | 8 | #include "lpf_1st.h" 9 | 10 | #ifndef M_PI 11 | #define M_PI 3.14159265358979323846 12 | #endif 13 | 14 | int lpf_1st_init(Lpf1stObj *filter, qfp_t fc, qfp_t fs) 15 | { 16 | if (!filter || fs <= 0) { 17 | return -1; 18 | } 19 | qfp_t ts = 1.0f / fs; 20 | filter->alpha = (2 * M_PI * ts * fc) / (1 + 2 * M_PI * ts * fc); 21 | filter->fc = fc; 22 | filter->ts = ts; 23 | filter->y_k1 = 0; 24 | return 0; 25 | } 26 | 27 | int lpf_1st_fc_set(Lpf1stObj *filter, qfp_t fc) 28 | { 29 | if (!filter || filter->ts <= 0) { 30 | return -1; 31 | } 32 | filter->alpha = (2 * M_PI * filter->ts * fc) / (1 + 2 * M_PI * filter->ts * fc); 33 | filter->fc = fc; 34 | return 0; 35 | } 36 | 37 | qfp_t lpf_1st_calc(Lpf1stObj *filter, qfp_t uk) 38 | { 39 | if(!filter) { 40 | return 0; 41 | } 42 | qfp_t y_k = filter->alpha * uk + (1 - filter->alpha) * filter->y_k1; 43 | filter->y_k1 = y_k; 44 | 45 | return y_k; 46 | } 47 | 48 | qfp_t lpf_1st_kcalc(Lpf1stObj *filter, qfp_t uk, qfp_t fs) 49 | { 50 | if(!filter || fs <= 0) { 51 | return 0; 52 | } 53 | qfp_t ts = 1.0f / fs; 54 | qfp_t alpha = (2 * M_PI * ts * filter->fc) / (1 + 2 * M_PI * ts * filter->fc); 55 | qfp_t y_k = alpha * uk + (1 - alpha) * filter->y_k1; 56 | filter->y_k1 = y_k; 57 | return y_k; 58 | } 59 | -------------------------------------------------------------------------------- /lpf_1st.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @Author: luoqi 3 | * @Date: 2022-03-16 09:26:15 4 | * @Last Modified by: luoqi 5 | * @Last Modified time: 2022-11-29 09:51:22 6 | */ 7 | 8 | #ifndef _LPF_1ST_H 9 | #define _LPF_1ST_H 10 | 11 | #ifdef __cplusplus 12 | extern "C" { 13 | #endif 14 | 15 | #ifndef qfp_t 16 | typedef float qfp_t; 17 | #endif 18 | 19 | typedef struct _lpf_first_order 20 | { 21 | qfp_t fc; // cut-off frequency 22 | qfp_t y_k1; // last output 23 | qfp_t alpha; // filter coefficient 24 | qfp_t ts; // samping period 25 | }Lpf1stObj; 26 | 27 | int lpf_1st_init(Lpf1stObj *filter, qfp_t fc, qfp_t fs); 28 | 29 | int lpf_1st_fc_set(Lpf1stObj *filter, qfp_t fc); 30 | 31 | /* ts is time-constant */ 32 | qfp_t lpf_1st_calc(Lpf1stObj *filter, qfp_t uk); 33 | 34 | /* ts is time-varying */ 35 | qfp_t lpf_1st_kcalc(Lpf1stObj *filter, qfp_t uk, qfp_t ts); 36 | 37 | #ifdef __cplusplus 38 | } 39 | #endif 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /maf.c: -------------------------------------------------------------------------------- 1 | /** 2 | * @ Author: luoqi 3 | * @ Create Time: 2024-07-22 15:37 4 | * @ Modified by: luoqi 5 | * @ Modified time: 2025-02-17 15:00 6 | * @ Description: 7 | */ 8 | 9 | #include "maf.h" 10 | 11 | int maf_init(LpfMaf *filter, qfp_t *buf, int wsize) 12 | { 13 | if(!filter || !buf || wsize <= 0) { 14 | return -1; 15 | } 16 | filter->wsize = wsize; 17 | filter->buf = buf; 18 | filter->sum = 0; 19 | filter->head = 0; 20 | return 0; 21 | } 22 | 23 | qfp_t maf_calc(LpfMaf *filter, qfp_t z) 24 | { 25 | if (!filter || !filter->buf || filter->wsize <= 0) { 26 | return 0; 27 | } 28 | filter->sum -= filter->buf[filter->head]; 29 | 30 | filter->buf[filter->head] = z; 31 | filter->sum += z; 32 | 33 | filter->head = (filter->head + 1) % filter->wsize; 34 | 35 | return filter->sum / (qfp_t)filter->wsize; 36 | } 37 | -------------------------------------------------------------------------------- /maf.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @ Author: luoqi 3 | * @ Create Time: 2024-07-22 15:37 4 | * @ Modified by: luoqi 5 | * @ Modified time: 2025-02-17 15:00 6 | * @ Description: 7 | */ 8 | 9 | #ifndef _MAF_H 10 | #define _MAF_H 11 | 12 | #ifdef __cplusplus 13 | extern "C" { 14 | #endif 15 | 16 | #ifndef qfp_t 17 | typedef float qfp_t; 18 | #endif 19 | 20 | typedef struct _lpf_sa 21 | { 22 | int wsize; 23 | int head; 24 | qfp_t sum; 25 | qfp_t *buf; 26 | } LpfMaf; 27 | 28 | int maf_init(LpfMaf *filter, qfp_t *buf, int wsize); 29 | 30 | qfp_t maf_calc(LpfMaf *filter, qfp_t z); 31 | 32 | #ifdef __cplusplus 33 | } 34 | #endif 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /notch_filter.c: -------------------------------------------------------------------------------- 1 | /** 2 | * @ Author: luoqi 3 | * @ Create Time: 2025-03-19 12:26 4 | * @ Modified by: luoqi 5 | * @ Modified time: 2025-03-19 21:11 6 | * @ Description: 7 | */ 8 | 9 | #include "notch_filter.h" 10 | 11 | #ifndef M_PI 12 | #define M_PI 3.14159265358979323846 13 | #endif 14 | 15 | static const qfp_t _fast_sin_table[91] = { 16 | 0.0, 0.017452406, 0.034899497, 0.052335956, 0.069756474, 17 | 0.087155743, 0.104528463, 0.121869343, 0.139173101, 0.156434465, 18 | 0.173648178, 0.190809, 0.207911, 0.224951, 0.241922, 19 | 0.258819, 0.275637, 0.292372, 0.309017, 0.325568, 20 | 0.342020, 0.358368, 0.374607, 0.390731, 0.406737, 21 | 0.422618, 0.438371, 0.453990, 0.469472, 0.48481, 22 | 0.5, 0.515038, 0.529919, 0.544639, 0.559193, 23 | 0.573576, 0.587785, 0.601815, 0.615661, 0.62932, 24 | 0.642788, 0.656059, 0.669131, 0.681998, 0.694658, 25 | 0.707107, 0.71934, 0.731354, 0.743145, 0.75471, 26 | 0.766044, 0.777146, 0.788011, 0.798636, 0.809017, 27 | 0.819152, 0.829038, 0.838671, 0.848048, 0.857167, 28 | 0.866025, 0.87462, 0.882948, 0.891007, 0.898794, 29 | 0.906308, 0.913545, 0.920505, 0.927184, 0.93358, 30 | 0.939693, 0.945519, 0.951057, 0.956305, 0.961262, 31 | 0.965926, 0.970296, 0.97437, 0.978148, 0.981627, 32 | 0.984808, 0.987688, 0.990268, 0.992546, 0.994522, 33 | 0.996195, 0.997564, 0.99863, 0.999391, 0.999848, 34 | 1.0 35 | }; 36 | 37 | static inline qfp_t _fmodf(qfp_t x, qfp_t y) 38 | { 39 | if(y == 0) { 40 | return NAN; 41 | } 42 | 43 | qfp_t abs_y = y < 0 ? -y : y; 44 | qfp_t sign_x = x < 0 ? -1 : 1; 45 | qfp_t abs_x = x < 0 ? -x : x; 46 | 47 | qfp_t div = abs_x / abs_y; 48 | qfp_t int_part = div >= 0 ? (qfp_t)((int)div) : -(qfp_t)((int)-div); 49 | 50 | qfp_t result = abs_x - (int_part * abs_y); 51 | 52 | return sign_x * result; 53 | } 54 | 55 | static qfp_t _fcos(qfp_t deg) 56 | { 57 | deg = _fmodf(deg + 90, 360); 58 | 59 | int sign = 1; 60 | // Use the periodic symmetry of sin 61 | if(deg > 180) { 62 | deg -= 180; 63 | sign = -1; 64 | } 65 | if(deg > 90) { 66 | deg = 180 - deg; 67 | } 68 | 69 | // Calculate the lookup table index and interpolation factor 70 | int index = (int)deg; // 1° resolution 71 | if(index >= 90) { 72 | return sign * _fast_sin_table[90]; 73 | } 74 | qfp_t fraction = deg - index; 75 | 76 | qfp_t sin_val = _fast_sin_table[index] * (1 - fraction) + _fast_sin_table[index + 1] * fraction; 77 | return sign * sin_val; 78 | } 79 | 80 | int notch_filter_init(NotchFilter *filter, qfp_t f0, qfp_t fs, qfp_t bw) 81 | { 82 | if(!filter || (f0 <= 0) || (fs <= 0) || (bw <= 0) || (bw >= fs / 2)) { 83 | return -1; 84 | } 85 | qfp_t r = 1 - 3 * bw / fs; 86 | qfp_t cos_w0 = _fcos(2 * M_PI * f0 / fs); 87 | filter->a1 = -2 * cos_w0 * r; 88 | filter->a2 = r * r; 89 | filter->b0 = 1; 90 | filter->b1 = -2 * cos_w0; 91 | filter->b2 = 1; 92 | 93 | filter->fs = fs; 94 | filter->f0 = f0; 95 | filter->bw = bw; 96 | 97 | filter->x1 = 0; 98 | filter->x2 = 0; 99 | filter->y1 = 0; 100 | filter->y2 = 0; 101 | 102 | return 0; 103 | } 104 | 105 | qfp_t notch_filter_calc(NotchFilter *filter, qfp_t x) 106 | { 107 | if(!filter) { 108 | return 0; 109 | } 110 | qfp_t y = filter->b0 * x + filter->b1 * filter->x1 + filter->b2 * filter->x2 - filter->a1 * filter->y1 - filter->a2 * filter->y2; 111 | filter->x2 = filter->x1; 112 | filter->x1 = x; 113 | filter->y2 = filter->y1; 114 | filter->y1 = y; 115 | return y; 116 | } 117 | 118 | int notch_filter_clr(NotchFilter *filter) 119 | { 120 | if(!filter) { 121 | return -1; 122 | } 123 | filter->x1 = 0; 124 | filter->x2 = 0; 125 | filter->y1 = 0; 126 | filter->y2 = 0; 127 | return 0; 128 | } 129 | 130 | int notch_filter_fs_set(NotchFilter *filter, qfp_t fs) 131 | { 132 | if(!filter || (fs <= 0)) { 133 | return -1; 134 | } 135 | filter->fs = fs; 136 | qfp_t r = 1 - 3 * filter->bw / fs; 137 | qfp_t cos_w0 = _fcos(2 * M_PI * filter->f0 / fs); 138 | filter->a1 = -2 * cos_w0 * r; 139 | filter->a2 = r * r; 140 | filter->b1 = -2 * cos_w0; 141 | filter->b2 = 1; 142 | return 0; 143 | } 144 | 145 | int notch_filter_bw_set(NotchFilter *filter, qfp_t bw) 146 | { 147 | if(!filter || (bw <= 0) || (bw >= filter->fs / 2)) { 148 | return -1; 149 | } 150 | filter->bw = bw; 151 | qfp_t r = 1 - 3 * bw / filter->fs; 152 | qfp_t cos_w0 = _fcos(2 * M_PI * filter->f0 / filter->fs); 153 | filter->a1 = -2 * cos_w0 * r; 154 | filter->a2 = r * r; 155 | filter->b1 = -2 * cos_w0; 156 | filter->b2 = 1; 157 | return 0; 158 | } 159 | 160 | int notch_filter_f0_set(NotchFilter *filter, qfp_t f0) 161 | { 162 | if(!filter || (f0 <= 0) || (f0 >= filter->fs / 2)) { 163 | return -1; 164 | } 165 | filter->f0 = f0; 166 | qfp_t r = 1 - 3 * filter->bw / filter->fs; 167 | qfp_t cos_w0 = _fcos(2 * M_PI * f0 / filter->fs); 168 | filter->a1 = -2 * cos_w0 * r; 169 | filter->a2 = r * r; 170 | filter->b1 = -2 * cos_w0; 171 | filter->b2 = 1; 172 | return 0; 173 | } 174 | -------------------------------------------------------------------------------- /notch_filter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @ Author: luoqi 3 | * @ Create Time: 2025-03-19 12:26 4 | * @ Modified by: luoqi 5 | * @ Modified time: 2025-03-19 21:12 6 | * @ Description: 7 | */ 8 | 9 | #ifndef _NOTCH_FILTER_H_ 10 | #define _NOTCH_FILTER_H_ 11 | 12 | #ifndef qfp_t 13 | typedef float qfp_t; 14 | #endif 15 | 16 | #ifndef NAN 17 | #define NAN (0.0f / 0.0f) 18 | #endif 19 | 20 | typedef struct { 21 | qfp_t a1; 22 | qfp_t a2; 23 | qfp_t b0; 24 | qfp_t b1; 25 | qfp_t b2; 26 | qfp_t x1; 27 | qfp_t x2; 28 | qfp_t y1; 29 | qfp_t y2; 30 | 31 | qfp_t fs; 32 | qfp_t bw; 33 | qfp_t f0; 34 | } NotchFilter; 35 | 36 | int notch_filter_init(NotchFilter *filter, qfp_t f0, qfp_t fs, qfp_t bw); 37 | 38 | qfp_t notch_filter_calc(NotchFilter *filter, qfp_t x); 39 | 40 | int notch_filter_clr(NotchFilter *filter); 41 | 42 | int notch_filter_fs_set(NotchFilter *filter, qfp_t fs); 43 | 44 | int notch_filter_bw_set(NotchFilter *filter, qfp_t bw); 45 | 46 | int notch_filter_f0_set(NotchFilter *filter, qfp_t f0); 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /pid.c: -------------------------------------------------------------------------------- 1 | /* 2 | * @Author: luoqi 3 | * @Date: 2021-04-27 19:20:38 4 | * @ Modified by: luoqi 5 | * @ Modified time: 2025-03-20 23:09 6 | */ 7 | 8 | #include "pid.h" 9 | 10 | static inline qfp_t _abs(qfp_t x) 11 | { 12 | return x > 0 ? x : -x; 13 | } 14 | 15 | static inline qfp_t update_pid_output(PidObj *pid) 16 | { 17 | if(pid->olimit != PID_NONE) { 18 | if(pid->y_k > pid->olimit) { 19 | pid->y_k = pid->olimit; 20 | } else if(pid->y_k < -pid->olimit) { 21 | pid->y_k = -pid->olimit; 22 | } 23 | } 24 | pid->y_k1 = pid->y_k; 25 | return pid->y_k; 26 | } 27 | 28 | int pid_init(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd, qfp_t olimit) 29 | { 30 | if(!pid) { 31 | return -1; 32 | } 33 | pid->delta_k = 0; 34 | pid->e_k1 = 0; 35 | pid->de_k1 = 0; 36 | pid->de_k2 = 0; 37 | pid->y_k1 = 0; 38 | pid->y_k = 0; 39 | pid->kp = kp; 40 | pid->ki = ki; 41 | pid->kd = kd; 42 | pid->alpha = 1; 43 | pid->kaw = ki; 44 | pid->nlo_k1 = 0; 45 | pid->olimit = olimit; 46 | return 0; 47 | } 48 | 49 | int pid_param_set(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd) 50 | { 51 | if(!pid) { 52 | return -1; 53 | } 54 | pid->kp = kp; 55 | pid->ki = ki; 56 | pid->kd = kd; 57 | return 0; 58 | } 59 | 60 | qfp_t pid_calc(PidObj *pid, qfp_t err, qfp_t dt) 61 | { 62 | if(!pid || dt < 0) { 63 | return -1; 64 | } 65 | qfp_t de_k = (dt != PID_NONE) ? (err - pid->e_k1) / dt : err - pid->e_k1; 66 | pid->delta_k = pid->kp * (err - pid->e_k1) 67 | + pid->ki * err 68 | + pid->kd * (de_k - pid->de_k1); 69 | 70 | pid->de_k1 = de_k; 71 | pid->e_k1 = err; 72 | pid->y_k = pid->y_k1 + pid->delta_k; 73 | 74 | return update_pid_output(pid); 75 | } 76 | 77 | qfp_t pid_aw_init(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd, qfp_t kaw, qfp_t olimit) 78 | { 79 | if(!pid) { 80 | return -1; 81 | } 82 | pid_init(pid, kp, ki, kd, olimit); 83 | pid->kaw = kaw; 84 | return 0; 85 | } 86 | 87 | qfp_t pid_aw_calc(PidObj *pid, qfp_t err, qfp_t dt) 88 | { 89 | if(!pid || dt < 0) { 90 | return -1; 91 | } 92 | 93 | // Calculate the derivative term (prevent dt from being 0 or special value) 94 | qfp_t de_k = (dt != PID_NONE) ? (err - pid->e_k1) / dt : err - pid->e_k1; 95 | 96 | // Calculate the p_term and derivative terms 97 | qfp_t p_term = pid->kp * (err - pid->e_k1); // Proportional term 98 | qfp_t d_term = pid->kd * (de_k - pid->de_k1); // Derivative term 99 | 100 | pid->delta_k = p_term + d_term + pid->ki * err - pid->kaw * (pid->nlo_k1 - pid->y_k1); 101 | pid->y_k = pid->y_k1 + pid->delta_k; 102 | pid->nlo_k1 = pid->y_k; 103 | pid->de_k1 = de_k; 104 | pid->e_k1 = err; 105 | 106 | return update_pid_output(pid); 107 | } 108 | 109 | int pid_integ_sep_init(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd, qfp_t alpha, qfp_t olimit) 110 | { 111 | if(!pid) { 112 | return -1; 113 | } 114 | pid_init(pid, kp, ki, kd, olimit); 115 | if(alpha < 0 || alpha > 1) { 116 | pid->alpha = 1; 117 | return -1; 118 | } 119 | pid->alpha = alpha; 120 | return 0; 121 | } 122 | 123 | qfp_t pid_integ_sep_calc(PidObj *pid, qfp_t err, qfp_t dt) 124 | { 125 | if(!pid || dt < 0) { 126 | return -1; 127 | } 128 | 129 | int th = _abs(err) > pid->alpha ? 0 : 1; // Threshold for integral separation 130 | qfp_t de_k = (dt != PID_NONE) ? (err - pid->e_k1) / dt : err - pid->e_k1; // Derivative of error 131 | 132 | // Calculate the control increment with integral separation 133 | pid->delta_k = pid->kp * (err - pid->e_k1) // Proportional term 134 | + th * pid->ki * err // Integral term with separation 135 | + pid->kd * (de_k - pid->de_k1); // Derivative term 136 | 137 | pid->de_k1 = de_k; // Update previous derivative of error 138 | pid->e_k1 = err; // Update previous error 139 | pid->y_k = pid->y_k1 + pid->delta_k; // Current output 140 | 141 | return update_pid_output(pid); 142 | } 143 | 144 | int pid_incplt_diff_init(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd, qfp_t alpha, qfp_t olimit) 145 | { 146 | if(!pid) { 147 | return -1; 148 | } 149 | 150 | pid_init(pid, kp, ki, kd, olimit); 151 | if((alpha < 0) || (alpha > 1)) { 152 | pid->alpha = 1; 153 | return -1; 154 | } 155 | pid->alpha = alpha; 156 | return 0; 157 | } 158 | 159 | qfp_t pid_incplt_diff_calc(PidObj *pid, qfp_t err, qfp_t dt) 160 | { 161 | if(!pid || dt < 0) { 162 | return -1; 163 | } 164 | 165 | qfp_t de_k = (dt != PID_NONE) ? (err - pid->e_k1) / dt : err - pid->e_k1; // Derivative of error 166 | 167 | // Calculate the control increment with incomplete differential 168 | pid->delta_k = pid->kp * (err - pid->e_k1) // Proportional term 169 | + pid->ki * err // Integral term 170 | + pid->kd * (pid->alpha * (de_k - 2 * pid->de_k1 + pid->de_k2) + pid->de_k1 - pid->de_k2); // Incomplete differential term 171 | 172 | pid->de_k2 = pid->de_k1; // Update previous previous derivative of error 173 | pid->de_k1 = de_k; // Update previous derivative of error 174 | pid->e_k1 = err; // Update previous error 175 | pid->y_k = pid->y_k1 + pid->delta_k; // Current output 176 | 177 | return update_pid_output(pid); 178 | } 179 | 180 | /* integral varible PID */ 181 | int pid_integ_var_init(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd, qfp_t l_th, qfp_t h_th, qfp_t olimit) 182 | { 183 | if(!pid) { 184 | return -1; 185 | } 186 | 187 | pid_init(pid, kp, ki, kd, olimit); 188 | 189 | if((l_th < 0) || (h_th < 0) || (l_th > h_th)) { 190 | pid->l_th = 1; 191 | return -1; 192 | } 193 | 194 | pid->l_th = l_th; 195 | pid->h_th = h_th; 196 | return 0; 197 | } 198 | 199 | qfp_t pid_integ_var_calc(PidObj *pid, qfp_t err, qfp_t dt) 200 | { 201 | if(!pid || dt < 0) { 202 | return -1; 203 | } 204 | 205 | qfp_t ratio = 0; 206 | qfp_t de_k = (dt != PID_NONE) ? (err - pid->e_k1) / dt : err - pid->e_k1; // Derivative of error 207 | 208 | // Calculate the ratio based on error magnitude for integral variable PID 209 | if(_abs(err) <= pid->l_th) { 210 | ratio = 1; 211 | } else if(_abs(err) > pid->h_th) { 212 | ratio = 0; 213 | } else { // l_th < |e| < h_th 214 | ratio = (pid->h_th - _abs(err)) / (pid->h_th - pid->l_th); 215 | } 216 | 217 | // Calculate the control increment with integral variable 218 | pid->delta_k = pid->kp * (err - pid->e_k1) // Proportional term 219 | + ratio * pid->ki * err // Integral term with variable scaling 220 | + pid->kd * (de_k - pid->de_k1); // Derivative term 221 | 222 | pid->de_k1 = de_k; // Update previous derivative of error 223 | pid->e_k1 = err; // Update previous error 224 | pid->y_k = pid->y_k1 + pid->delta_k; // Current output 225 | 226 | return update_pid_output(pid); 227 | } 228 | 229 | int pid_diff_first_init(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd, qfp_t alpha, qfp_t olimit) 230 | { 231 | if(!pid) { 232 | return -1; 233 | } 234 | 235 | pid_init(pid, kp, ki, kd, olimit); 236 | if(alpha < 0 || alpha > 1) { 237 | pid->alpha = 0; 238 | return -1; 239 | } 240 | pid->alpha = alpha; 241 | return 0; 242 | } 243 | 244 | qfp_t pid_diff_first_calc(PidObj *pid, qfp_t err, qfp_t dt) 245 | { 246 | if (!pid || dt < 0) { 247 | return -1; 248 | } 249 | 250 | qfp_t de_k = (dt != PID_NONE) ? (err - pid->e_k1) / dt : err - pid->e_k1; // Derivative of error 251 | 252 | qfp_t prev_ey_k1 = pid->ey_k1; 253 | qfp_t prev_ey_k2 = pid->ey_k2; 254 | 255 | qfp_t p_term = de_k; 256 | qfp_t i_term = pid->ki * err; 257 | 258 | qfp_t d_term = pid->kd * (pid->alpha * (pid->delta_k - 2 * prev_ey_k1 + prev_ey_k2) + (1 - pid->alpha) * prev_ey_k1); 259 | 260 | pid->delta_k = p_term + i_term + d_term; 261 | 262 | pid->ey_k2 = prev_ey_k1; 263 | pid->ey_k1 = pid->delta_k; 264 | 265 | pid->e_k1 = err; 266 | pid->y_k = pid->y_k1 + pid->delta_k; 267 | 268 | return update_pid_output(pid); 269 | } 270 | 271 | /* incomplete differential and integral varible PID */ 272 | int pid_incplt_diff_integ_var_init(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd, qfp_t alpha, qfp_t l_th, qfp_t h_th, qfp_t olimit) 273 | { 274 | if(!pid) { 275 | return -1; 276 | } 277 | 278 | pid_init(pid, kp, ki, kd, olimit); 279 | if((alpha < 0) || (alpha > 1)) { 280 | pid->alpha = 1; 281 | return -1; 282 | } 283 | pid->alpha = alpha; 284 | if((l_th < 0) || (h_th < 0) || (l_th > h_th)) { 285 | pid->l_th = 1; 286 | return -1; 287 | } 288 | pid->l_th = l_th; 289 | pid->h_th = h_th; 290 | return 0; 291 | } 292 | 293 | qfp_t pid_incplt_diff_integ_var_calc(PidObj *pid, qfp_t err, qfp_t dt) 294 | { 295 | if(!pid || dt < 0) { 296 | return -1; 297 | } 298 | 299 | qfp_t de_k = (dt != PID_NONE) ? (err - pid->e_k1) / dt : err - pid->e_k1; // Derivative of error 300 | qfp_t ratio = 0; 301 | 302 | // Calculate the ratio based on error magnitude for integral variable PID 303 | if(_abs(err) <= pid->l_th) { 304 | ratio = 1; 305 | } else if(_abs(err) > pid->h_th) { 306 | ratio = 0; 307 | } else { // l_th < |e| < h_th 308 | ratio = (pid->h_th - _abs(err)) / (pid->h_th - pid->l_th); 309 | } 310 | 311 | // Calculate the control increment with incomplete differential and integral variable 312 | pid->delta_k = pid->kp * de_k // Proportional term 313 | + ratio * pid->ki * err // Integral term with variable scaling 314 | + pid->kd * (pid->alpha * (de_k - 2 * pid->de_k1 + pid->de_k2) + pid->de_k1 - pid->de_k2); // Incomplete differential term 315 | 316 | pid->de_k2 = pid->de_k1; // Update previous previous derivative of error 317 | pid->de_k1 = de_k; // Update previous derivative of error 318 | pid->e_k1 = err; // Update previous error 319 | pid->y_k = pid->y_k1 + pid->delta_k; // Current output 320 | 321 | return update_pid_output(pid); 322 | } 323 | 324 | /* differential first and integral varible PID */ 325 | int pid_diff_first_integ_var_init(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd, qfp_t alpha, qfp_t l_th, qfp_t h_th, qfp_t olimit) 326 | { 327 | if(!pid) { 328 | return -1; 329 | } 330 | 331 | pid_init(pid, kp, ki, kd, olimit); 332 | if((alpha < 0) || (alpha > 1)) { 333 | pid->alpha = 1; 334 | return -1; 335 | } 336 | pid->alpha = alpha; 337 | if((l_th < 0) || (h_th < 0) || (l_th > h_th)) { 338 | pid->l_th = 1; 339 | return -1; 340 | } 341 | pid->l_th = l_th; 342 | pid->h_th = h_th; 343 | return 0; 344 | } 345 | 346 | qfp_t pid_diff_first_integ_var_calc(PidObj *pid, qfp_t err, qfp_t dt) 347 | { 348 | if (!pid || dt < 0) { 349 | return -1; 350 | } 351 | 352 | qfp_t de_k = (dt != PID_NONE) ? (err - pid->e_k1) / dt : err - pid->e_k1; 353 | 354 | qfp_t ratio = 0; 355 | if (pid->h_th == pid->l_th) { 356 | ratio = (_abs(err) <= pid->l_th) ? 1 : 0; 357 | } else if (_abs(err) <= pid->l_th) { 358 | ratio = 1; 359 | } else if (_abs(err) > pid->h_th) { 360 | ratio = 0; 361 | } else { 362 | ratio = (pid->h_th - _abs(err)) / (pid->h_th - pid->l_th); 363 | } 364 | 365 | qfp_t prev_ey_k1 = pid->ey_k1; 366 | qfp_t prev_ey_k2 = pid->ey_k2; 367 | 368 | qfp_t p_term = pid->kp * de_k; 369 | qfp_t i_term = ratio * pid->ki * err; 370 | qfp_t d_term = pid->kd * (pid->alpha * (pid->delta_k - 2 * prev_ey_k1 + prev_ey_k2) + (1 - pid->alpha) * prev_ey_k1); 371 | 372 | pid->delta_k = p_term + i_term + d_term; 373 | 374 | pid->ey_k2 = prev_ey_k1; 375 | pid->ey_k1 = pid->delta_k; 376 | pid->de_k2 = pid->de_k1; 377 | pid->de_k1 = de_k; 378 | 379 | pid->e_k1 = err; 380 | pid->y_k = pid->y_k1 + pid->delta_k; 381 | 382 | return update_pid_output(pid); 383 | } 384 | 385 | int pid_clr(PidObj *pid) 386 | { 387 | if(!pid) { 388 | return -1; 389 | } 390 | 391 | pid->y_k = 0; 392 | pid->y_k1 = 0; 393 | pid->delta_k = 0; 394 | pid->nlo_k1 = 0; 395 | pid->e_k1 = 0; 396 | pid->de_k1 = 0; 397 | pid->de_k2 = 0; 398 | pid->ey_k1 = 0; 399 | pid->ey_k2 = 0; 400 | return 0; 401 | } 402 | 403 | int pid_calc_clr(PidObj *pid) 404 | { 405 | if(!pid) { 406 | return -1; 407 | } 408 | 409 | pid->delta_k = 0; 410 | pid->e_k1 = 0; 411 | pid->nlo_k1 = 0; 412 | pid->de_k1 = 0; 413 | pid->de_k2 = 0; 414 | pid->ey_k1 = 0; 415 | pid->ey_k2 = 0; 416 | return 0; 417 | } 418 | -------------------------------------------------------------------------------- /pid.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @Author: luoqi 3 | * @Date: 2021-04-27 19:19:44 4 | * @Last Modified by: luoqi 5 | * @Last Modified time: 2022-01-26 17:03:39 6 | */ 7 | 8 | #ifndef _PID_H 9 | #define _PID_H 10 | 11 | #ifdef __cplusplus 12 | extern "C" 13 | { 14 | #endif 15 | 16 | #ifndef qfp_t 17 | typedef float qfp_t; 18 | #endif 19 | 20 | // Structure definition for PID controller 21 | typedef struct pid_structure 22 | { 23 | qfp_t kp; // Proportional gain coefficient 24 | qfp_t ki; // Integral gain coefficient 25 | qfp_t kd; // Derivative gain coefficient 26 | qfp_t kaw; // Anti-windup gain coefficient 27 | qfp_t nlo_k1; // No limit output at the previous time step 28 | qfp_t e_k1; // Error at the previous time step 29 | qfp_t de_k1; // Derivative of error at the previous time step 30 | qfp_t y_k; // Current output 31 | qfp_t y_k1; // Output at the previous time step 32 | qfp_t delta_k; // Output increment 33 | 34 | // Used in incomplete differential PID 35 | qfp_t de_k2; // Derivative of error at the previous previous time step 36 | // Used in incomplete differential or integral separation threshold 37 | qfp_t alpha; // 0 < alpha < 1 38 | 39 | // Used in integral variable PID 40 | qfp_t l_th; // Low threshold for integral variable PID 41 | qfp_t h_th; // High threshold for integral variable PID 42 | 43 | // Used in differential first PID 44 | qfp_t ey_k1; // Derivative of output at the previous time step 45 | qfp_t ey_k2; // Derivative of output at the previous previous time step 46 | 47 | qfp_t olimit; // Output limit 48 | } PidObj; 49 | 50 | #define PID_NONE (0) 51 | 52 | // Initialize a basic PID controller 53 | int pid_init(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd, qfp_t olimit); 54 | 55 | // Set parameters for a PID controller 56 | int pid_param_set(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd); 57 | 58 | // Calculate output for a basic PID controller 59 | qfp_t pid_calc(PidObj *pid, qfp_t err, qfp_t dt); 60 | 61 | // Initialize an anti-windup PID controller with back-calculation method 62 | qfp_t pid_aw_init(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd, qfp_t kaw, qfp_t olimit); 63 | 64 | // Calculate output for an anti-windup PID controller 65 | qfp_t pid_aw_calc(PidObj *pid, qfp_t err, qfp_t dt); 66 | 67 | // Initialize an incomplete differential PID controller 68 | int pid_incplt_diff_init(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd, qfp_t alpha, qfp_t olimit); 69 | 70 | // Calculate output for an incomplete differential PID controller 71 | qfp_t pid_incplt_diff_calc(PidObj *ctrl, qfp_t err, qfp_t dt); 72 | 73 | // Initialize an integral separation PID controller 74 | int pid_integ_sep_init(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd, qfp_t alpha, qfp_t olimit); 75 | 76 | // Calculate output for an integral separation PID controller 77 | qfp_t pid_integ_sep_calc(PidObj *pid, qfp_t err, qfp_t dt); 78 | 79 | // Initialize an integral variable PID controller 80 | int pid_integ_var_init(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd, qfp_t l_th, qfp_t h_th, qfp_t olimit); 81 | 82 | // Calculate output for an integral variable PID controller 83 | qfp_t pid_integ_var_calc(PidObj *pid, qfp_t err, qfp_t dt); 84 | 85 | // Initialize a differential first PID controller 86 | int pid_diff_first_init(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd, qfp_t alpha, qfp_t olimit); 87 | 88 | // Calculate output for a differential first PID controller 89 | qfp_t pid_diff_first_calc(PidObj *pid, qfp_t err, qfp_t dt); 90 | 91 | // Initialize an incomplete differential and integral variable PID controller 92 | int pid_incplt_diff_integ_var_init(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd, qfp_t alpha, qfp_t lth, qfp_t hth, qfp_t olimit); 93 | 94 | // Calculate output for an incomplete differential and integral variable PID controller 95 | qfp_t pid_incplt_diff_integ_var_calc(PidObj *ctrl, qfp_t err, qfp_t dt); 96 | 97 | // Initialize a differential first and integral variable PID controller 98 | int pid_diff_first_integ_var_init(PidObj *pid, qfp_t kp, qfp_t ki, qfp_t kd, qfp_t alpha, qfp_t lth, qfp_t hth, qfp_t olimit); 99 | 100 | // Calculate output for a differential first and integral variable PID controller 101 | qfp_t pid_diff_first_integ_var_calc(PidObj *pid, qfp_t err, qfp_t dt); 102 | 103 | // Clear the PID controller's state 104 | int pid_clr(PidObj *pid); 105 | 106 | // Clear the PID controller's state and calculate output 107 | int pid_calc_clr(PidObj *pid); 108 | 109 | #ifdef __cplusplus 110 | } 111 | #endif 112 | #endif 113 | -------------------------------------------------------------------------------- /qfoc.c: -------------------------------------------------------------------------------- 1 | /** 2 | * @ Author: luoqi 3 | * @ Create Time: 2024-08-02 10:15 4 | * @ Modified by: luoqi 5 | * @ Modified time: 2025-05-30 01:47 6 | * @ Description: 7 | */ 8 | 9 | #include "qfoc.h" 10 | 11 | static inline qfp_t _clamp(qfp_t val, qfp_t min, qfp_t max) 12 | { 13 | return (val > max) ? max : ((val < min) ? min : val); 14 | } 15 | 16 | static void *_memset(void *dest, int c, uint32_t n) 17 | { 18 | if(!dest) { 19 | return QNULL; 20 | } 21 | 22 | if(n == 0) { 23 | return dest; 24 | } 25 | 26 | uint8_t *pdest = (uint8_t *)dest; 27 | uint8_t value = (uint8_t)c; 28 | 29 | if(n >= 4 && ((uintptr_t)pdest & 3) == 0) { 30 | uint32_t fill = value | (value << 8) | (value << 16) | (value << 24); 31 | uint32_t *pdest32 = (uint32_t *)pdest; 32 | 33 | while(n >= 4) { 34 | *pdest32++ = fill; 35 | n -= 4; 36 | } 37 | 38 | pdest = (uint8_t *)pdest32; 39 | } 40 | 41 | while(n--) { 42 | *pdest++ = value; 43 | } 44 | 45 | return dest; 46 | } 47 | 48 | static inline qfp_t _rpm2deg(qfp_t rpm) 49 | { 50 | return 6 * rpm; 51 | } 52 | 53 | static inline qfp_t _deg2rpm(qfp_t deg) 54 | { 55 | return deg / 6; 56 | } 57 | 58 | static inline qfp_t _fmodf(qfp_t x, qfp_t y) 59 | { 60 | if(y == 0) { 61 | return NAN; 62 | } 63 | 64 | qfp_t abs_y = y < 0 ? -y : y; 65 | qfp_t sign_x = x < 0 ? -1 : 1; 66 | qfp_t abs_x = x < 0 ? -x : x; 67 | 68 | qfp_t div = abs_x / abs_y; 69 | qfp_t int_part = div >= 0 ? (qfp_t)((int)div) : -(qfp_t)((int)-div); 70 | 71 | qfp_t result = abs_x - (int_part * abs_y); 72 | 73 | return sign_x * result; 74 | } 75 | 76 | // static const qfp_t _QFOC_PI = 3.141592653589793f; 77 | // static const qfp_t _QFOC_SQRT3 = 1.732050807568877f; 78 | // static const qfp_t _QFOC_SQRT3_BY_3 = 0.577350269189626f; 79 | // static const qfp_t _QFOC_3_BY_2 = 1.5f; 80 | static const qfp_t _QFOC_ONE_BY_SQRT3 = 0.577350269189626f; 81 | static const qfp_t _QFOC_TWO_BY_SQRT3 = 1.154700538379252f; 82 | static const qfp_t _QFOC_SQRT3_BY_2 = 0.866025403784439f; 83 | static const qfp_t _QFOC_2_BY_3 = 0.666666666666667f; 84 | 85 | static inline qfp_t _abs(qfp_t x) 86 | { 87 | return (x >= 0) ? x : -x; 88 | } 89 | 90 | static inline qfp_t _max(qfp_t x, qfp_t y) 91 | { 92 | return (x >= y) ? x : y; 93 | } 94 | 95 | static inline qfp_t _min(qfp_t x, qfp_t y) 96 | { 97 | return (x <= y) ? x : y; 98 | } 99 | 100 | #define _DEG2RAD (0.017453292519943295769236907684886f) 101 | 102 | static const qfp_t _fast_sin_table[91] = { 103 | 0.0, 0.017452406, 0.034899497, 0.052335956, 0.069756474, 104 | 0.087155743, 0.104528463, 0.121869343, 0.139173101, 0.156434465, 105 | 0.173648178, 0.190809, 0.207911, 0.224951, 0.241922, 106 | 0.258819, 0.275637, 0.292372, 0.309017, 0.325568, 107 | 0.342020, 0.358368, 0.374607, 0.390731, 0.406737, 108 | 0.422618, 0.438371, 0.453990, 0.469472, 0.48481, 109 | 0.5, 0.515038, 0.529919, 0.544639, 0.559193, 110 | 0.573576, 0.587785, 0.601815, 0.615661, 0.62932, 111 | 0.642788, 0.656059, 0.669131, 0.681998, 0.694658, 112 | 0.707107, 0.71934, 0.731354, 0.743145, 0.75471, 113 | 0.766044, 0.777146, 0.788011, 0.798636, 0.809017, 114 | 0.819152, 0.829038, 0.838671, 0.848048, 0.857167, 115 | 0.866025, 0.87462, 0.882948, 0.891007, 0.898794, 116 | 0.906308, 0.913545, 0.920505, 0.927184, 0.93358, 117 | 0.939693, 0.945519, 0.951057, 0.956305, 0.961262, 118 | 0.965926, 0.970296, 0.97437, 0.978148, 0.981627, 119 | 0.984808, 0.987688, 0.990268, 0.992546, 0.994522, 120 | 0.996195, 0.997564, 0.99863, 0.999391, 0.999848, 121 | 1.0 122 | }; 123 | 124 | static qfp_t _fatan2(qfp_t y, qfp_t x) 125 | { 126 | // a := min (|x|, |y|) / max (|x|, |y|) 127 | qfp_t abs_y = _abs(y); 128 | qfp_t abs_x = _abs(x); 129 | // inject FLT_MIN in denominator to avoid division by zero 130 | qfp_t a = _min(abs_x, abs_y) / (_max(abs_x, abs_y) + 1.17549e-38); 131 | // s := a * a 132 | qfp_t s = a * a; 133 | // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a 134 | qfp_t r = ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a; 135 | // if |y| > |x| then r := 1.57079637 - r 136 | if(abs_y > abs_x) { 137 | r = 1.57079637f - r; 138 | } 139 | // if x < 0 then r := 3.14159274 - r 140 | if(x < 0) { 141 | r = 3.14159274f - r; 142 | } 143 | // if y < 0 then r := -r 144 | if(y < 0) { 145 | r = -r; 146 | } 147 | 148 | return r; 149 | } 150 | 151 | static qfp_t _fsin(qfp_t deg) 152 | { 153 | deg = _fmodf(deg, 360); 154 | 155 | int sign = 1; 156 | // Use the periodic symmetry of sin 157 | if(deg > 180) { 158 | deg -= 180; 159 | sign = -1; 160 | } 161 | if(deg > 90) { 162 | deg = 180 - deg; 163 | } 164 | 165 | // Calculate the lookup table index and interpolation factor 166 | int index = (int)deg; // 1° resolution 167 | if(index >= 90) { 168 | return sign * _fast_sin_table[90]; 169 | } 170 | qfp_t fraction = deg - index; 171 | 172 | qfp_t sin_val = _fast_sin_table[index] * (1 - fraction) + _fast_sin_table[index + 1] * fraction; 173 | return sign * sin_val; 174 | } 175 | 176 | static inline qfp_t _fcos(qfp_t deg) 177 | { 178 | return _fsin(deg + 90); 179 | } 180 | 181 | static inline qfp_t _fsqrt(qfp_t x) 182 | { 183 | union { 184 | qfp_t f; 185 | int i; 186 | } conv; 187 | qfp_t x2; 188 | x2 = x * 0.5f; 189 | conv.f = x; 190 | conv.i = 0x5f3504f3 - (conv.i >> 1); 191 | x = conv.f; 192 | x = x * (1.5f - (x2 * x * x)); // 1st iteration 193 | x = x * (1.5f - (x2 * x * x)); // 2nd iteration 194 | 195 | return 1 / x; 196 | } 197 | 198 | static int _svm(qfp_t alpha, qfp_t beta, qfp_t *ta, qfp_t *tb, qfp_t *tc) 199 | { 200 | if(!ta || !tb || !tc) { 201 | return -1; 202 | } 203 | uint8_t sector = 0; 204 | if(beta >= 0) { 205 | if(alpha >= 0) { 206 | if(alpha < beta * _QFOC_ONE_BY_SQRT3) { 207 | sector = 2; 208 | } else { 209 | sector = 1; 210 | } 211 | } else { 212 | if(alpha < -beta * _QFOC_ONE_BY_SQRT3) { 213 | sector = 3; 214 | } else { 215 | sector = 2; 216 | } 217 | } 218 | } else { 219 | if(alpha >= 0) { 220 | if(alpha < -beta * _QFOC_ONE_BY_SQRT3) { 221 | sector = 5; 222 | } else { 223 | sector = 6; 224 | } 225 | } else { 226 | if(alpha < beta * _QFOC_ONE_BY_SQRT3) { 227 | sector = 4; 228 | 229 | } else { 230 | sector = 5; 231 | } 232 | } 233 | } 234 | qfp_t t1 = 0, t2 = 0; 235 | switch(sector) { 236 | case 1: 237 | t1 = alpha - _QFOC_ONE_BY_SQRT3 * beta; 238 | t2 = _QFOC_TWO_BY_SQRT3 * beta; 239 | *ta = (1 - t1 - t2) * 0.5f; 240 | *tb = *ta + t1; 241 | *tc = *tb + t2; 242 | break; 243 | case 2: 244 | t1 = alpha + _QFOC_ONE_BY_SQRT3 * beta; 245 | t2 = -alpha + _QFOC_ONE_BY_SQRT3 * beta; 246 | *tb = (1 - t1 - t2) * 0.5f; 247 | *ta = *tb + t2; 248 | *tc = *ta + t1; 249 | break; 250 | case 3: 251 | t1 = _QFOC_TWO_BY_SQRT3 * beta; 252 | t2 = -alpha - _QFOC_ONE_BY_SQRT3 * beta; 253 | *tb = (1 - t1 - t2) * 0.5f; 254 | *tc = *tb + t1; 255 | *ta = *tc + t2; 256 | break; 257 | case 4: 258 | t1 = -alpha + _QFOC_ONE_BY_SQRT3 * beta; 259 | t2 = -_QFOC_TWO_BY_SQRT3 * beta; 260 | *tc = (1 - t1 - t2) * 0.5f; 261 | *tb = *tc + t2; 262 | *ta = *tb + t1; 263 | break; 264 | case 5: 265 | t1 = -alpha - _QFOC_ONE_BY_SQRT3 * beta; 266 | t2 = alpha - _QFOC_ONE_BY_SQRT3 * beta; 267 | *tc = (1 - t1 - t2) * 0.5f; 268 | *ta = *tc + t1; 269 | *tb = *ta + t2; 270 | break; 271 | case 6: 272 | t1 = -_QFOC_TWO_BY_SQRT3 * beta; 273 | t2 = alpha + _QFOC_ONE_BY_SQRT3 * beta; 274 | *ta = (1 - t1 - t2) * 0.5f; 275 | *tc = *ta + t2; 276 | *tb = *tc + t1; 277 | break; 278 | default: 279 | *ta = 0; 280 | *tb = 0; 281 | *tc = 0; 282 | return -1; 283 | } 284 | return sector; 285 | } 286 | 287 | static inline int _clarke_transform(qfp_t a, qfp_t b, qfp_t c, qfp_t *alpha, qfp_t *beta) 288 | { 289 | if(!alpha || !beta) { 290 | return -1; 291 | } 292 | *alpha = (a - 0.5f * (b + c)) * _QFOC_2_BY_3; 293 | *beta = (_QFOC_SQRT3_BY_2 * (b - c)) * _QFOC_2_BY_3; 294 | return 0; 295 | } 296 | 297 | static inline int _iclarke_transform(qfp_t alpha, qfp_t beta, qfp_t *a, qfp_t *b, qfp_t *c) 298 | { 299 | if(!a || !b || !c) { 300 | return -1; 301 | } 302 | *a = alpha; 303 | *b = -0.5 * alpha + _QFOC_SQRT3_BY_2 * beta; 304 | *c = -0.5 * alpha - _QFOC_SQRT3_BY_2 * beta; 305 | return 0; 306 | } 307 | 308 | static inline int _park_transform(qfp_t alpha, qfp_t beta, qfp_t edegree, qfp_t *q, qfp_t *d) 309 | { 310 | if(!q || !d) { 311 | return -1; 312 | } 313 | *q = -alpha * _fsin(edegree) + beta * _fcos(edegree); 314 | *d = alpha * _fcos(edegree) + beta * _fsin(edegree); 315 | return 0; 316 | } 317 | 318 | static inline int _ipark_transform(qfp_t q, qfp_t d, qfp_t edegree, qfp_t *alpha, qfp_t *beta) 319 | { 320 | if(!alpha || !beta) { 321 | return -1; 322 | } 323 | *alpha = d * _fcos(edegree) - q * _fsin(edegree); 324 | *beta = d * _fsin(edegree) + q * _fcos(edegree); 325 | return 0; 326 | } 327 | 328 | static int _qsvm_calc(qfp_t vbus, qfp_t vq, qfp_t vd, qfp_t edegree, qfp_t *ta, qfp_t *tb, qfp_t *tc) 329 | { 330 | if(!ta || !tb || !tc) { 331 | return -1; 332 | } 333 | qfp_t alpha, beta; 334 | vq = (vq > vbus) ? vbus : ((vq < -vbus) ? -vbus : vq); 335 | vd = (vd > vbus) ? vbus : ((vd < -vbus) ? -vbus : vd); 336 | qfp_t _vq = (vq / vbus) * _QFOC_SQRT3_BY_2; 337 | qfp_t _vd = (vd / vbus) * _QFOC_SQRT3_BY_2; 338 | _ipark_transform(_vq, _vd, edegree, &alpha, &beta); 339 | return _svm(alpha, beta, ta, tb, tc); 340 | } 341 | 342 | 343 | 344 | int qfoc_init(QFocObj *foc, PmsmMotor *motor, uint16_t pwm_max, qfp_t vbus_max, qfp_t cilimit, uint32_t i2t_times, qfp_t imax, qfp_t vel_max, qfp_t pos_max, qfp_t pos_min) 345 | { 346 | if(!foc || !motor) { 347 | return -1; 348 | } 349 | int ret = 0; 350 | _memset(foc, 0, sizeof(QFocObj)); 351 | foc->motor = motor; 352 | if(foc->motor->gear_ratio <= 0) { 353 | foc->motor->gear_ratio = 1; 354 | foc->status = QFOC_STATUS_ERROR; 355 | foc->err = QFOC_ERR_MOTOR_PARAM;; 356 | ret = -1; 357 | } 358 | if(foc->motor->poles_pairs < 1) { 359 | foc->motor->poles_pairs = 1; 360 | foc->status = QFOC_STATUS_ERROR; 361 | foc->err = QFOC_ERR_MOTOR_PARAM;; 362 | ret = -1; 363 | } 364 | if(foc->motor->rated_current <= 0) { 365 | foc->motor->rated_current = 0; 366 | foc->status = QFOC_STATUS_ERROR; 367 | foc->err = QFOC_ERR_MOTOR_PARAM;; 368 | ret = -1; 369 | } 370 | if(foc->motor->rated_voltage <= 0) { 371 | foc->motor->rated_voltage = 0; 372 | foc->status = QFOC_STATUS_ERROR; 373 | foc->err = QFOC_ERR_MOTOR_PARAM;; 374 | ret = -1; 375 | } 376 | if(foc->motor->kt <= 0) { 377 | foc->motor->kt = 0; 378 | foc->status = QFOC_STATUS_ERROR; 379 | foc->err = QFOC_ERR_MOTOR_PARAM; 380 | ret = -1; 381 | } 382 | foc->cilimit = cilimit; 383 | foc->i2t_times = i2t_times; 384 | foc->pwm_max = pwm_max; 385 | foc->vbus_max = vbus_max; 386 | foc->vel_max = vel_max; 387 | foc->pos_max = pos_max; 388 | foc->pos_min = pos_min; 389 | if(imax <= 0) { 390 | foc->imax = 0; 391 | foc->iphase_max = 0; 392 | foc->status = QFOC_STATUS_ERROR; 393 | foc->err = QFOC_ERR_IMAX_NOT_SET; 394 | ret = -1; 395 | } else { 396 | foc->imax = imax; 397 | foc->iphase_max = imax; 398 | } 399 | return ret; 400 | } 401 | 402 | int qfoc_iloopc_set(QFocObj *foc, QFocController controller) 403 | { 404 | if(!foc || !controller) { 405 | return -1; 406 | } 407 | 408 | foc->iloop_controller = controller; 409 | return 0; 410 | } 411 | 412 | int qfoc_ploopc_set(QFocObj *foc, QFocController controller) 413 | { 414 | if(!foc || !controller) { 415 | return -1; 416 | } 417 | foc->ploop_controller = controller; 418 | return 0; 419 | } 420 | 421 | int qfoc_vloopc_set(QFocObj *foc, QFocController controller) 422 | { 423 | if(!foc || !controller) { 424 | return -1; 425 | } 426 | foc->vloop_controller = controller; 427 | return 0; 428 | } 429 | 430 | int qfoc_enable(QFocObj *foc, QFocEnable ena) 431 | { 432 | if(!foc) { 433 | return -1; 434 | } 435 | if(ena == QFOC_ENABLE) { 436 | foc->iqref = 0; 437 | foc->idref = 0; 438 | foc->vref = 0; 439 | foc->pref = foc->pos; 440 | foc->status = QFOC_STATUS_RUNNING; 441 | foc->err = QFOC_ERR_NONE; 442 | } else { 443 | foc->status = QFOC_STATUS_IDLE; 444 | } 445 | 446 | return 0; 447 | } 448 | 449 | int qfoc_vbus_update(QFocObj *foc, qfp_t vbus) 450 | { 451 | if(!foc) { 452 | return -1; 453 | } 454 | if(vbus > foc->vbus_max) { 455 | foc->vbus = foc->vbus_max; 456 | foc->err = QFOC_ERR_OVBUS; 457 | } else { 458 | foc->vbus = vbus; 459 | } 460 | return 0; 461 | } 462 | 463 | int qfoc_iabc_update(QFocObj *foc, qfp_t ia, qfp_t ib, qfp_t ic) 464 | { 465 | if(!foc) { 466 | return -1; 467 | } 468 | qfp_t alpha, beta; 469 | foc->ia = ia; 470 | foc->ib = ib; 471 | foc->ic = ic; 472 | 473 | if((_abs(ia) > foc->iphase_max) || (_abs(ib) > foc->iphase_max) || (_abs(ic) > foc->iphase_max)) { 474 | foc->err = QFOC_ERR_OIMAX; 475 | return -1; 476 | } 477 | 478 | _clarke_transform(ia, ib, ic, &alpha, &beta); 479 | 480 | if(foc->i2t_cnt++ > foc->i2t_times) { 481 | foc->ci = foc->ipower / foc->i2t_times; 482 | foc->i2t_cnt = 0; 483 | foc->ipower = 0; 484 | if(foc->ci > foc->cilimit) { 485 | foc->err = QFOC_ERR_OPWR; 486 | } 487 | } else { 488 | foc->ipower += _fsqrt(foc->iq * foc->iq + foc->id * foc->id); 489 | } 490 | 491 | _park_transform(alpha, beta, foc->edegree, &foc->iq, &foc->id); 492 | if((foc->iq > foc->imax) || (foc->iq < -foc->imax) || (foc->id > foc->imax) || (foc->id < -foc->imax)) { 493 | foc->err = QFOC_ERR_OIMAX; 494 | return -1; 495 | } 496 | return 0; 497 | } 498 | 499 | int qfoc_vel_update(QFocObj *foc, qfp_t vel) 500 | { 501 | if(!foc) { 502 | return -1; 503 | } 504 | foc->vel = vel; 505 | if(foc->vel_max != QFOC_NO_LIMIT) { 506 | if((vel > foc->vel_max) || (vel < -foc->vel_max)) { 507 | foc->err = QFOC_ERR_OVMAX; 508 | return -1; 509 | } 510 | } 511 | return 0; 512 | } 513 | 514 | int qfoc_epos_update(QFocObj *foc, qfp_t epos) 515 | { 516 | if(!foc || !foc->motor || (foc->motor->gear_ratio <= 0)) { 517 | foc->status = QFOC_STATUS_ERROR; 518 | foc->err = QFOC_ERR_MOTOR_PARAM; 519 | return -1; 520 | } 521 | foc->epos = epos; 522 | foc->pos = epos / foc->motor->gear_ratio; 523 | 524 | if((foc->pos_max != QFOC_NO_LIMIT) || (foc->pos_min != QFOC_NO_LIMIT)) { 525 | if((foc->pos > foc->pos_max) || (foc->pos < foc->pos_min)) { 526 | if(foc->pos > foc->pos_max) { 527 | foc->err = QFOC_ERR_OPMAX; 528 | } else { 529 | foc->err = QFOC_ERR_OPMIN; 530 | } 531 | return -1; 532 | } 533 | } 534 | foc->edegree = _fmodf(epos * foc->motor->poles_pairs, 360); 535 | if(foc->edegree < 0) { 536 | foc->edegree += 360; 537 | } 538 | return 0; 539 | } 540 | 541 | /* FOC controller reference input set */ 542 | int qfoc_vqd_set(QFocObj *foc, qfp_t vq, qfp_t vd) 543 | { 544 | if(!foc) { 545 | return -1; 546 | } 547 | foc->vq = _clamp(vq, -foc->vbus, foc->vbus); 548 | foc->vd = _clamp(vd, -foc->vbus, foc->vbus); 549 | return 0; 550 | } 551 | 552 | int qfoc_iref_set(QFocObj *foc, qfp_t iqref, qfp_t idref) 553 | { 554 | if(!foc) { 555 | return -1; 556 | } 557 | foc->iqref = _clamp(iqref, -foc->imax, foc->imax); 558 | foc->idref = _clamp(idref, -foc->imax, foc->imax); 559 | return 0; 560 | } 561 | 562 | int qfoc_vref_set(QFocObj *foc, qfp_t vref) 563 | { 564 | if(!foc) { 565 | return -1; 566 | } 567 | if(foc->vel_max != QFOC_NO_LIMIT) { 568 | foc->vref = _clamp(vref, -foc->vel_max, foc->vel_max); 569 | } else { 570 | foc->vref = vref; 571 | } 572 | return 0; 573 | } 574 | 575 | int qfoc_pref_set(QFocObj *foc, qfp_t pref) 576 | { 577 | if(!foc) { 578 | return -1; 579 | } 580 | if((foc->pos_max == QFOC_NO_LIMIT) && (foc->pos_min == QFOC_NO_LIMIT)) { 581 | foc->pref = pref; 582 | } else { 583 | foc->pref = _clamp(pref, foc->pos_min, foc->pos_max); 584 | } 585 | return 0; 586 | } 587 | 588 | int qfoc_force_calc(qfp_t vbus, qfp_t vq, qfp_t vd, qfp_t edegree, uint16_t pwm_max, uint16_t *pwma, uint16_t *pwmb, uint16_t *pwmc) 589 | { 590 | qfp_t ta, tb, tc; 591 | 592 | edegree = _fmodf(edegree, 360); 593 | edegree = (edegree < 0) ? edegree + 360 : edegree; 594 | 595 | int sector = _qsvm_calc(vbus, vq, vd, edegree, &ta, &tb, &tc); 596 | *pwma = (uint16_t)(ta * pwm_max); 597 | *pwmb = (uint16_t)(tb * pwm_max); 598 | *pwmc = (uint16_t)(tc * pwm_max); 599 | return sector; 600 | } 601 | 602 | int qfoc_oloop_update(QFocObj *foc, uint16_t *pwma, uint16_t *pwmb, uint16_t *pwmc) 603 | { 604 | if(!foc) { 605 | return -1; 606 | } 607 | qfp_t ta, tb, tc; 608 | 609 | if(foc->status != QFOC_STATUS_RUNNING) { 610 | *pwma = 0; 611 | *pwmb = 0; 612 | *pwmc = 0; 613 | return -1; 614 | } 615 | if(foc->err != QFOC_ERR_NONE) { 616 | foc->status = QFOC_STATUS_ERROR; 617 | *pwma = 0; 618 | *pwmb = 0; 619 | *pwmc = 0; 620 | return -1; 621 | } 622 | 623 | foc->sector = _qsvm_calc(foc->vbus, foc->vq, foc->vd, foc->edegree, &ta, &tb, &tc); 624 | foc->pwma = (uint16_t)(ta * foc->pwm_max); 625 | foc->pwmb = (uint16_t)(tb * foc->pwm_max); 626 | foc->pwmc = (uint16_t)(tc * foc->pwm_max); 627 | 628 | *pwma = foc->pwma; 629 | *pwmb = foc->pwmb; 630 | *pwmc = foc->pwmc; 631 | return 0; 632 | } 633 | 634 | /* FOC current loop control */ 635 | int qfoc_iloop_update(QFocObj *foc, uint16_t *pwma, uint16_t *pwmb, uint16_t *pwmc) 636 | { 637 | if(!foc) { 638 | return -1; 639 | } 640 | qfp_t ta, tb, tc; 641 | 642 | if(foc->status != QFOC_STATUS_RUNNING) { 643 | *pwma = 0; 644 | *pwmb = 0; 645 | *pwmc = 0; 646 | return 0; 647 | } 648 | if(foc->err != QFOC_ERR_NONE) { 649 | foc->status = QFOC_STATUS_ERROR; 650 | *pwma = 0; 651 | *pwmb = 0; 652 | *pwmc = 0; 653 | return -1; 654 | } 655 | QFocOutput output = { 0 }; 656 | QFocError err = foc->iloop_controller(foc, &output); 657 | 658 | if(err != QFOC_ERR_NONE) { 659 | foc->status = QFOC_STATUS_ERROR; 660 | foc->err = err; 661 | *pwma = 0; 662 | *pwmb = 0; 663 | *pwmc = 0; 664 | return -1; 665 | } 666 | 667 | foc->sector = _qsvm_calc(foc->vbus, output.vq, output.vd, foc->edegree, &ta, &tb, &tc); 668 | foc->pwma = (uint16_t)(ta * foc->pwm_max); 669 | foc->pwmb = (uint16_t)(tb * foc->pwm_max); 670 | foc->pwmc = (uint16_t)(tc * foc->pwm_max); 671 | 672 | *pwma = foc->pwma > foc->pwm_max ? foc->pwm_max : foc->pwma; 673 | *pwmb = foc->pwmb > foc->pwm_max ? foc->pwm_max : foc->pwmb; 674 | *pwmc = foc->pwmc > foc->pwm_max ? foc->pwm_max : foc->pwmc; 675 | return 0; 676 | } 677 | 678 | /* FOC velocity/position current double loop pid control */ 679 | int qfoc_vloop_update(QFocObj *foc) 680 | { 681 | if(!foc) { 682 | return -1; 683 | } 684 | QFocOutput output = { 0 }; 685 | foc->err = foc->vloop_controller(foc, &output); 686 | 687 | if(output.to == QFOC_OUT_TO_ILOOP) { 688 | foc->iqref = output.iqref; 689 | foc->idref = output.idref; 690 | } else if(output.to == QFOC_OUT_TO_OLOOP) { 691 | foc->vq = output.vq; 692 | foc->vd = output.vd; 693 | } else { 694 | foc->err = QFOC_ERR_LOOP_OUTPUT; 695 | return -1; 696 | } 697 | 698 | return 0; 699 | } 700 | 701 | int qfoc_ploop_update(QFocObj *foc) 702 | { 703 | if(!foc) { 704 | return -1; 705 | } 706 | QFocOutput output = { 0 }; 707 | foc->err = foc->ploop_controller(foc, &output); 708 | 709 | if(output.to == QFOC_OUT_TO_VLOOP) { 710 | foc->vref = output.vref; 711 | } else if(output.to == QFOC_OUT_TO_ILOOP) { 712 | foc->iqref = output.iqref; 713 | foc->idref = output.idref; 714 | } else if(output.to == QFOC_OUT_TO_OLOOP) { 715 | foc->vq = output.vq; 716 | foc->vd = output.vd; 717 | } else { 718 | foc->err = QFOC_ERR_LOOP_OUTPUT; 719 | return -1; 720 | } 721 | return 0; 722 | } 723 | 724 | int qfoc_calib_calc(QFocObj *foc, qfp_t vdmax, qfp_t pref, uint16_t *pwma, uint16_t *pwmb, uint16_t *pwmc) 725 | { 726 | if(!foc || !pwma || !pwmb || !pwmc) { 727 | return -1; 728 | } 729 | qfp_t ta, tb, tc; 730 | qfp_t edegree = _fmodf(pref * foc->motor->poles_pairs, 360); 731 | foc->sector = _qsvm_calc(foc->vbus, 0, vdmax, edegree, &ta, &tb, &tc); 732 | *pwma = (uint16_t)(ta * foc->pwm_max); 733 | *pwmb = (uint16_t)(tb * foc->pwm_max); 734 | *pwmc = (uint16_t)(tc * foc->pwm_max); 735 | return 0; 736 | } 737 | -------------------------------------------------------------------------------- /qfoc.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @ Author: luoqi 3 | * @ Create Time: 2024-08-02 10:15 4 | * @ Modified by: luoqi 5 | * @ Modified time: 2025-05-30 01:53 6 | * @ Description: 7 | */ 8 | 9 | #ifndef _QFOC_H 10 | #define _QFOC_H 11 | 12 | #include 13 | 14 | /** 15 | define qfoc float data type, 16 | user can redefine thier own float type 17 | to guaranty precision 18 | */ 19 | #ifndef qfp_t 20 | typedef float qfp_t; 21 | #endif 22 | 23 | #ifndef QNULL 24 | #define QNULL ((void *)0) 25 | #endif 26 | 27 | #ifndef NAN 28 | #define NAN (0.0f / 0.0f) 29 | #endif 30 | 31 | /** 32 | * @brief: Structure defining the parameters of a PMSM motor. 33 | * Includes pole pairs, phase resistance, inductance, rated values, and gear ratio. 34 | */ 35 | typedef struct { 36 | uint8_t poles_pairs; // Number of pole pairs 37 | qfp_t phase_resistance; // Phase resistance in Ohms 38 | qfp_t phase_inductance; // Phase inductance in mH 39 | 40 | qfp_t rated_current; // Rated current in Amperes 41 | qfp_t stall_current; // Stall current in Amperes 42 | qfp_t rated_torque; // Rated torque in Nm 43 | qfp_t stall_torque; // Stall torque in Nm 44 | qfp_t rated_voltage; // Rated voltage in Volts 45 | qfp_t rated_speed; // Rated speed in RPM 46 | 47 | qfp_t kt; // Torque constant in Nm/A 48 | qfp_t gear_ratio; // Gear ratio 49 | } PmsmMotor; 50 | 51 | /** 52 | * @brief: Enum defining the enable states for FOC control. 53 | */ 54 | typedef enum { 55 | QFOC_DISABLE = 0, // Disable FOC 56 | QFOC_ENABLE, // Enable FOC 57 | } QFocEnable; 58 | 59 | /** 60 | * @brief: Enum defining the status of the FOC system. 61 | */ 62 | typedef enum { 63 | QFOC_STATUS_IDLE = 0, // Idle state 64 | QFOC_STATUS_RUNNING, // Running state 65 | QFOC_STATUS_ERROR, // Error state 66 | } QFocStatus; 67 | 68 | /** 69 | * @brief: Enum defining possible errors in the FOC system. 70 | */ 71 | typedef enum { 72 | QFOC_ERR_NONE = 0, // No error 73 | QFOC_ERR_OBJ_NOT_FOUND = -1, // FOC objects not found 74 | QFOC_ERR_IMAX_NOT_SET = -2, // Maximum current limit not set 75 | QFOC_ERR_OIMAX = -3, // Over current limit error 76 | QFOC_ERR_OVMAX = -4, // Over velocity limit error 77 | QFOC_ERR_OPMAX = -5, // Over position max limit error 78 | QFOC_ERR_OPMIN = -6, // Over position min limit error 79 | QFOC_ERR_OVBUS = -7, // Over vbus limit error 80 | QFOC_ERR_OPWR = -8, // Over power limit error 81 | QFOC_ERR_MOTOR_PARAM = -9, // Motor parameter error 82 | QFOC_ERR_OITRACK = -10, // Over current tracking limit error 83 | QFOC_ERR_OVTRACK = -11, // Over velocity tracking limit error 84 | QFOC_ERR_OPTRACK = -12, // Over position tracking limit error 85 | QFOC_ERR_PHASE = -13, // Phase error 86 | QFOC_ERR_LOOP_OUTPUT = -14, // Loop output error 87 | } QFocError; 88 | 89 | /** 90 | * @brief: Enum defining the output target for FOC calculations 91 | * Used to determine the next processing stage in the FOC control chain 92 | */ 93 | typedef enum { 94 | QFOC_OUT_TO_NONE = 0, // No output 95 | QFOC_OUT_TO_OLOOP, // Output to open loop update stage 96 | QFOC_OUT_TO_ILOOP, // Output to i loop control stage 97 | QFOC_OUT_TO_VLOOP, // Output to v loop control stage 98 | } QFocOutputTo; 99 | 100 | /** 101 | * @brief: Structure holding FOC control loop parameters and references 102 | * Contains all the necessary reference values and intermediate results 103 | * for FOC control calculations across different control loops 104 | */ 105 | typedef struct { 106 | qfp_t iqref; // Reference current for q-axis in Amperes 107 | qfp_t idref; // Reference current for d-axis in Amperes 108 | qfp_t vref; // Reference velocity in degrees/second 109 | qfp_t pref; // Reference position in degrees 110 | qfp_t vq; // Calculated voltage for q-axis in Volts 111 | qfp_t vd; // Calculated voltage for d-axis in Volts 112 | QFocOutputTo to; // Specifies the next processing stage in control chain 113 | } QFocOutput; 114 | 115 | /** 116 | * @brief: Main structure representing the FOC object. 117 | * Contains all parameters, references, outputs, and controllers for FOC operation. 118 | */ 119 | typedef struct _qfoc { 120 | QFocStatus status; // Current status of the FOC system 121 | QFocError err; // Error code of the FOC system 122 | qfp_t epos; // Encoder position in degrees 123 | qfp_t pos; // Position in degrees (epos * gear_ratio) 124 | qfp_t vel; // Velocity in degrees/second 125 | qfp_t iq; // Current for q-axis in Amperes 126 | qfp_t id; // Current for d-axis in Amperes 127 | qfp_t ia; // Phase A current in Amperes 128 | qfp_t ib; // Phase B current in Amperes 129 | qfp_t ic; // Phase C current in Amperes 130 | qfp_t edegree; // Electrical degree (0-360 degrees) 131 | qfp_t vbus; // Power supply voltage in Volts 132 | 133 | qfp_t pref; // Reference position in degrees 134 | qfp_t vref; // Reference velocity in degrees/second 135 | qfp_t iqref; // Reference current for q-axis in Amperes 136 | qfp_t idref; // Reference current for d-axis in Amperes 137 | qfp_t vq; // Voltage for q-axis in Volts 138 | qfp_t vd; // Voltage for d-axis in Volts 139 | 140 | uint16_t pwm_max; // Maximum PWM output value 141 | uint16_t pwma; // PWM output for phase A 142 | uint16_t pwmb; // PWM output for phase B 143 | uint16_t pwmc; // PWM output for phase C 144 | uint8_t sector; // SVM space vector sector (1-6) 145 | 146 | qfp_t imax; // Current limit for q-axis and d-axis in Amperes 147 | qfp_t iphase_max; // Phase current limit in Amperes 148 | qfp_t vel_max; // Velocity limit in degrees/second 149 | qfp_t pos_max; // Maximum position limit in degrees 150 | qfp_t pos_min; // Minimum position limit in degrees 151 | qfp_t vbus_max; // Maximum power supply voltage in Volts 152 | qfp_t cilimit; // Continuous current limit for i2t calculation in Amperes 153 | qfp_t ci; // Continuous current in Amperes 154 | qfp_t ipower; // Average power over a period 155 | uint32_t i2t_times; // i2t integral times 156 | uint32_t i2t_cnt; // Integral counter 157 | PmsmMotor *motor; // Pointer to the motor object 158 | 159 | QFocError (*iloop_controller)(const struct _qfoc *foc, QFocOutput *output); // Current loop controller 160 | QFocError (*ploop_controller)(const struct _qfoc *foc, QFocOutput *output); // Position loop controller 161 | QFocError (*vloop_controller)(const struct _qfoc *foc, QFocOutput *output); // Velocity loop controller 162 | } QFocObj; 163 | 164 | #define QFOC_NO_LIMIT 0 165 | 166 | typedef QFocError (*QFocController)(const QFocObj *foc, QFocOutput *output); 167 | 168 | /** 169 | * @brief: Initializes the FOC object with motor parameters and limits. 170 | * @param foc: Pointer to the FOC object. 171 | * @param motor: Pointer to the motor object. 172 | * @param pwm_max: Maximum PWM output value. 173 | * @param vbus_max: Maximum power supply voltage in Volts. 174 | * @param cilimit: Continuous current limit for i2t calculation in Amperes. 175 | * @param i2t_times: Number of i2t integral times. 176 | * @param imax: Current limit for q-axis in Amperes. 177 | * @param vel_max: Velocity limit in degrees/second. 178 | * @param pos_max: Maximum position limit in degrees. 179 | * @param pos_min: Minimum position limit in degrees. 180 | * @return: Status of initialization. 181 | */ 182 | int qfoc_init(QFocObj *foc, PmsmMotor *motor, uint16_t pwm_max, qfp_t vbus_max, qfp_t cilimit, uint32_t i2t_times, qfp_t imax, qfp_t vel_max, qfp_t pos_max, qfp_t pos_min); 183 | 184 | /** 185 | * @brief Sets the current loop controller for FOC. 186 | * Users need to implement the current loop algorithm and register it using this function. 187 | * The registered controller will be automatically called during the FOC loop calculations. 188 | * 189 | * @param foc Pointer to the FOC object. 190 | * @param controller Function pointer to the user-defined current loop controller. 191 | * The controller should calculate target iq and id values based on the FOC parameters. 192 | * @return 0 on success, -1 on failed. 193 | */ 194 | int qfoc_iloopc_set(QFocObj *foc, QFocController controller); 195 | 196 | /** 197 | * @brief: Sets the position loop controller for FOC. 198 | * Users need to implement the position loop algorithm and register it using this function. 199 | * The registered controller will be automatically called during the FOC loop calculations. 200 | * @param foc: Pointer to the FOC object. 201 | * @param controller: Function pointer to the user-defined position loop controller. 202 | * @return: 0 on success, -1 on failed. 203 | */ 204 | int qfoc_ploopc_set(QFocObj *foc, QFocController controller); 205 | 206 | /** 207 | * @brief: Sets the velocity loop controller for FOC. 208 | * Users need to implement the velocity loop algorithm and register it using this function. 209 | * The registered controller will be automatically called during the FOC loop calculations. 210 | * @param foc: Pointer to the FOC object. 211 | * @param controller: Function pointer to the user-defined velocity loop controller. 212 | * @return: 0 on success, -1 on failed. 213 | */ 214 | int qfoc_vloopc_set(QFocObj *foc, QFocController controller); 215 | 216 | /** 217 | * @brief: Enables or disables the FOC system. 218 | * @param foc: Pointer to the FOC object. 219 | * @param ena: Enable state (QFOC_ENABLE or QFOC_DISABLE). 220 | * @return: 0 on success, -1 on failed. 221 | */ 222 | int qfoc_enable(QFocObj *foc, QFocEnable ena); 223 | 224 | /** 225 | * @brief: Updates the FOC system with the latest vbus voltage value. 226 | * @param foc: Pointer to the FOC object. 227 | * @param vbus: Latest vbus voltage value. 228 | * @return: 0 on success, -1 on failed. 229 | */ 230 | int qfoc_vbus_update(QFocObj *foc, qfp_t vbus); 231 | 232 | /** 233 | * @brief: Updates the FOC system with the latest phase currents. 234 | * @param foc: Pointer to the FOC object. 235 | * @param ia: Phase A current. 236 | * @param ib: Phase B current. 237 | * @param ic: Phase C current. 238 | * @return: 0 on success, -1 on failed. 239 | */ 240 | int qfoc_iabc_update(QFocObj *foc, qfp_t ia, qfp_t ib, qfp_t ic); 241 | 242 | /** 243 | * @brief: Updates the FOC system with the latest velocity value. 244 | * @param foc: Pointer to the FOC object. 245 | * @param vel: Latest velocity value. 246 | * @return: 0 on success, -1 on failed. 247 | */ 248 | int qfoc_vel_update(QFocObj *foc, qfp_t vel); 249 | 250 | /** 251 | * @brief: Updates the FOC system with the latest encoder position. 252 | * @param foc: Pointer to the FOC object. 253 | * @param epos: Latest encoder position. 254 | * @return: 0 on success, -1 on failed. 255 | */ 256 | int qfoc_epos_update(QFocObj *foc, qfp_t epos); 257 | 258 | /** 259 | * @brief: Sets the target voltages for the q-axis and d-axis in the FOC system. 260 | * @param foc: Pointer to the FOC object. 261 | * @param vq: Target voltage for the q-axis. 262 | * @param vd: Target voltage for the d-axis. 263 | * @return: 0 on success, -1 on failed. 264 | */ 265 | int qfoc_vqd_set(QFocObj *foc, qfp_t vq, qfp_t vd); 266 | 267 | /** 268 | * @brief: Sets the reference currents for the q-axis and d-axis in the FOC system. 269 | * @param foc: Pointer to the FOC object. 270 | * @param iqref: Reference current for the q-axis. 271 | * @param idref: Reference current for the d-axis. 272 | * @return: 0 on success, -1 on failed. 273 | */ 274 | int qfoc_iref_set(QFocObj *foc, qfp_t iqref, qfp_t idref); 275 | 276 | /** 277 | * @brief: Sets the reference velocity for the FOC system. 278 | * @param foc: Pointer to the FOC object. 279 | * @param vref: Reference velocity. 280 | * @return: 0 on success, -1 on failed. 281 | */ 282 | int qfoc_vref_set(QFocObj *foc, qfp_t vref); 283 | 284 | /** 285 | * @brief: Sets the reference position for the FOC system. 286 | * @param foc: Pointer to the FOC object. 287 | * @param pref: Reference position. 288 | * @return: 0 on success, -1 on failed. 289 | */ 290 | int qfoc_pref_set(QFocObj *foc, qfp_t pref); 291 | 292 | /** 293 | * @brief: Calculates the SVM sector and PWM outputs for a given electrical degree and voltages. 294 | * @param vbus: Bus voltage. 295 | * @param vq: Voltage for the q-axis. 296 | * @param vd: Voltage for the d-axis. 297 | * @param edegree: Electrical degree. 298 | * @param pwm_max: Maximum PWM output value. 299 | * @param pwma: Pointer to store PWM output for phase A. 300 | * @param pwmb: Pointer to store PWM output for phase B. 301 | * @param pwmc: Pointer to store PWM output for phase C. 302 | * @return: 0 on success, -1 on failed. 303 | */ 304 | int qfoc_force_calc(qfp_t vbus, qfp_t vq, qfp_t vd, qfp_t edegree, uint16_t pwm_max, uint16_t *pwma, uint16_t *pwmb, uint16_t *pwmc); 305 | 306 | /** 307 | * @brief: Updates the FOC system in open-loop mode. 308 | * @param foc: Pointer to the FOC object. 309 | * @param pwma: Pointer to store PWM output for phase A. 310 | * @param pwmb: Pointer to store PWM output for phase B. 311 | * @param pwmc: Pointer to store PWM output for phase C. 312 | * @return: 0 on success, -1 on failed. 313 | */ 314 | int qfoc_oloop_update(QFocObj *foc, uint16_t *pwma, uint16_t *pwmb, uint16_t *pwmc); 315 | 316 | /** 317 | * @brief: Updates the FOC system in current loop mode. 318 | * @param foc: Pointer to the FOC object. 319 | * @param pwma: Pointer to store PWM output for phase A. 320 | * @param pwmb: Pointer to store PWM output for phase B. 321 | * @param pwmc: Pointer to store PWM output for phase C. 322 | * @return: 0 on success, -1 on failed. 323 | */ 324 | int qfoc_iloop_update(QFocObj *foc, uint16_t *pwma, uint16_t *pwmb, uint16_t *pwmc); 325 | 326 | /** 327 | * @brief: Updates the FOC system in velocity loop mode. 328 | * @param foc: Pointer to the FOC object. 329 | * @return: 0 on success, -1 on failed. 330 | */ 331 | int qfoc_vloop_update(QFocObj *foc); 332 | 333 | /** 334 | * @brief: Updates the FOC system in position loop mode. 335 | * @param foc: Pointer to the FOC object. 336 | * @return: 0 on success, -1 on failed. 337 | */ 338 | int qfoc_ploop_update(QFocObj *foc); 339 | 340 | /** 341 | * @brief: Performs phase calibration to align the d-axis with phase A. 342 | * This function should be called first, followed by a delay to record the encoder position. 343 | * The recorded position represents the phase bias and can also be used for encoder calibration. 344 | * @param foc: Pointer to the FOC object. 345 | * @param vdmax: Maximum voltage for the d-axis. 346 | * @param pref: Reference position. 347 | * @param pwma: Pointer to store PWM output for phase A. 348 | * @param pwmb: Pointer to store PWM output for phase B. 349 | * @param pwmc: Pointer to store PWM output for phase C. 350 | * @return: 0 on success, -1 on failed. 351 | */ 352 | int qfoc_calib_calc(QFocObj *foc, qfp_t vdmax, qfp_t pref, uint16_t *pwma, uint16_t *pwmb, uint16_t *pwmc); 353 | 354 | #endif 355 | --------------------------------------------------------------------------------