定点数锁相环实现思路与复盘
评论 0 热度 546
标题中提到的锁相环为三相锁相环，原理见此：三相锁相环（一）SRF-PLL算法参数设计、时域分析、频域分析与simulink仿真验证，本文不多赘述。
背景
现在很多锁相环很多是基于浮点数实现的，在一些高端的DSP上运算速度尚且可以，而我在项目中用的16位单片机（dsPIC33EP）性能太弱鸡，单次的浮点运算就需要3us左右，网侧电压频率又很高（400~800Hz），使用浮点数计算根本跟不上网侧电压变化速度，因此必须通过其他方法来加速运算。
芯片对于数学计算的支持
经过对编译器文件一通翻找，我发现厂家对芯片提供了定点数数学计算库（libQ），可以很方便地计算正弦、余弦、开方等计算，同时（DSP库）也提供了向量、矩阵运算和加速版PID控制器支持。其运算输入和输出都是以定点数表示，因此我转去看了看定点数，在MCU仿真中测试了一下运算速度，发现比浮点数快了30+倍。初略计算后发现使用定点数全部替换浮点数计算后基本可以满足要求。
顺带一提，这里提到的两个库的定点数本质是int16类型，不是_Fract、_Accum定点类型。定点类型我只听说过，但一直没有用过。
探寻器件对定点数类型的支持
一是库中并没有提供单独两个定点数的运算支持，只提供了向量之间的运算（虽然这也可以对两个定点数进行计算），二是此前在翻阅芯片配套编译器手册时，我记得芯片支持语言层的定点数类型运算（即_Fract/_Accum类型），我认为这种更加规范的类型编译后在器件上可能会对运算有一定优化，因此决定探索一下器件的定点数类型。
在XC16编译器的手册上，我找到了器件的定点格式以及C数据类型。但是文中只是简单解释了一下类型和对应的定点数据格式，甚至没有解释如何打开这个定点数数据类型的开关，只是在编译器手册中简单提到要打开一个compiler switch，后来我在MPLAB中鼓捣了好久才发现如何打开。
在开启定点数数据类型支持后，新的问题又来了：
- 我需要用到的其他库里面提供的数学计算支持，但是只能计算本质为int16类型的定点数，对于语言层面的定点数数据类型无法正常计算。
- 器件不支持定点类型与int16（表示的定点数）之间的计算，也就是定点类型只能与定点类型进行基本计算（我记得这个是规范中规定的）。
如果要强制将定点类型与int16之间进行计算，那么数据类型会被转换，也就是定点类型会被转化int16类型，其转换结果永远为-1/0/1，造成计算结果是错误的。这一开始令我非常困惑，但是后来想了想其实这与浮点数转换成整数的逻辑是一样的，这样的设计也算是比较科学规范的。
作为探索的补充，把定点类型和int16之间进行转换的思路如下：以定点转int16定点数为例，先把定点类型取地址并转化为void类型指针，再转化为int16指针，最后再取值，即：int b = *(int *) (void *) &a。
我无法接受转换过程带来的时间消耗，在权衡之下只得放弃器件的定点数数据类型，选择直接用普通的int16实现定点数的基本计算，然后将器件未提供的一些基本运算用C实现。
由于int16定点数相当于把普通的数字扩大了2^15倍（32768），所以将两个定点数相乘后再缩小2^15得到的就是以定点数表示的乘积结果，在C代码中只需要先把两个int16转换为int32，然后再进行基本运算，最后右移15位转换为int16即可。
锁相环实现
因为需要遵循器件库函数输入要求、ADC精度不高、纯int32计算太慢，我的定点数采用int16数据类型，遵循Q15格式，即1个最高位用来表示符号，后面15位用来表示大小，这种格式可以表示范围在-1到1-2^-15（约0.99997，下文中近似为1）。
由于Q15定点数的表示范围限制，需要对模型进行修改，逐步定点化。
- 在simulink仿真中搭建好模型之后，将网侧输入适当缩放并留足空间，使其输入值始终在-1~1中。
- 修改PI控制器的参数，也确保其值在-1~1中。注意，在下图中，我的PI控制器输出的值为$\frac{\omega}{f_s \pi}$，而不是$\omega$，这样可以使得PI控制器的参数较好设置，同时积分后输出的结果在-1~1内，分别对应到-π~π。
- 将模型内部各个模块的数据格式逐步设置为
fixdt(1, 16, 15)，在MATLAB Function 模块中使用fi(x, 1, 16, 15)将运算中的数据转化为定点数，确保计算过程用的是定点数，个别难处理的地方可以酌情考虑用double算完后再转化为定点数。 - 修改积分器，使得积分结果不会溢出。我使用的策略是直接积分，如果积分结果大于等于
fi(1, 1, 16, 15)，则将积分结果替换为fi(-1, 1, 16, 15)；如果积分结果小于等于fi(-1, 1, 16, 15)，则替换为fi(1, 1, 16, 15)。注意：此处积分器使用的是饱和加法，当两个定点数的和会溢出时，必定会返回定点数所能表示的最大值或最小值。再次注意：当两个fixdt(1, 16, 15)定点数和溢出时，matlab返回的不是旧格式了，而是fixdt(1, 17, 15)！
下图为PLL模型图，输入为网侧三相电压，输出theta为角度，此角度范围为[-1, 1]，映射到[-π, π]；输出Em为幅值。
下面是代码（非芯片上的代码，仅仅是在PC上仿真）。为了在PC上测试代码，我把芯片独有的一些库去掉（libQ和DSP库等用于辅助定点数计算的库），改用C模拟实现了，并写在头文件fit.h中，一并粘贴在文末。对于cos、sin、sqrt的计算我并未用查表法实现，而是偷偷用math库过度了，因此需要移植到其他单片机上时，还需要对代码进行修改。
#include "stdio.h"
#include "stdint.h"
#include "fit.h"
#include "stdlib.h"
FILE *f;
#define fs 40e3
// 直接测量得到的网侧电压
// 倍数：1/330
fractional mGridAVolt = 0;
fractional mGridBVolt = 0;
fractional mGridCVolt = 0;
// 经过锁相得到的电压 倍数同mGridAVolt
fractional mPLLVabc[3] = {0, 0, 0};
#define mVa mGridAVolt
#define mVb mGridBVolt
#define mVc mGridCVolt
#define pVa mPLLVabc[0]
#define pVb mPLLVabc[1]
#define pVc mPLLVabc[2]
#define Va Vabc[0]
#define Vb Vabc[1]
#define Vc Vabc[2]
// theta 角度 范围-1~1-2^-15 映射到-pi~pi
static fractional theta;
// 网侧电压幅值 范围：0 ~ 1-2^-15 与mGridAVolt同幅值 即1/200
fractional gridVoltEm = 0;
// 频率 但是值为omega/fs/pi，即频率(Hz)为omega*fs/2，fs为采样频率
fractional gridVoltFreq = 0;
static fractional PLLKPID[3] = {
floatToFract(0.19894),
floatToFract(0.0049736),
};
static fractional __PLLKPIDSpaceDelay[] = {0, 0};
static tPID PLLKPIDController = {
.PIparam = PLLKPID,
.controlHistory = __PLLKPIDSpaceDelay,
.controlReference = 0,
.measuredOutput = 0,
};
void PLL_step(void)
{
fractional Vabc[3] = {0, 0, 0};
static fractional TClarkeAlpha[3] = {Q15Const_2d3, -Q15Const_1d3, -Q15Const_1d3};
static fractional TClarkebeta[2] = {Q15Const_1dsqrt3, -Q15Const_1dsqrt3};
Va = (mVa > 0) ? mVa : pVa;
Vb = (mVb > 0) ? mVb : pVb;
Vc = (mVc > 0) ? mVc : pVc;
// ---------------- Clarke Transform ----------------
// Xalpha, Xbeta
fractional Xclarke[2];
Xclarke[0] = VectorDotProduct(3, TClarkeAlpha, Vabc);
Xclarke[1] = VectorDotProduct(2, TClarkebeta, Vabc + 1);
// ------------------ Park Transform ------------------
// Xd, Xq
fractional Xpark[2];
fractional TparkCos = _Q15cosPI(theta);
fractional TparkSin = _Q15sinPI(theta);
fractional TparkD[2] = {TparkCos, TparkSin};
fractional TparkQ[2] = {-TparkSin, TparkCos};
Xpark[0] = VectorDotProduct(2, TparkD, Xclarke);
Xpark[1] = VectorDotProduct(2, TparkQ, Xclarke);
// ----------------- 幅值计算 --------------------------
gridVoltEm = _Q15sqrt(VectorPower(2, Xpark));
if (gridVoltEm < Q15Const_0)
{
gridVoltEm = Q15Const_0;
}
// ----------------- 控制器作用 ----------------------
// Xq作为控制器输入
PLLKPIDController.controlReference = Xpark[1];
PID(&PLLKPIDController);
gridVoltFreq = PLLKPIDController.controlOutput;
// ---------------- 积分得到theta角度 -------------
VectorAdd(1, &theta, &theta, &gridVoltFreq);
// theta上饱和 达到1，减去2
if (theta == Q15Const_1)
{
// 注意 -1不是 -Q15Const_1
theta = Q15Const_n1;
}
else if (theta == Q15Const_n1)
{
theta = Q15Const_1;
}
// ---------------- 正弦信号重构 --------------------
// 避免溢出 分批次加或减2
fractional thetaTmp[3];
if (theta < -Q15Const_1d3)
{
thetaTmp[1] = theta + Q15Const_1;
thetaTmp[1] = thetaTmp[1] - Q15Const_2d3;
thetaTmp[1] = thetaTmp[1] + Q15Const_1;
}
else
{
thetaTmp[1] = theta - Q15Const_2d3;
}
if (theta > Q15Const_1d3)
{
thetaTmp[2] = theta - Q15Const_1;
thetaTmp[2] = thetaTmp[2] + Q15Const_2d3;
thetaTmp[2] = thetaTmp[2] - Q15Const_1;
}
else
{
thetaTmp[2] = theta + Q15Const_2d3;
}
thetaTmp[0] = _Q15cosPI(theta);
thetaTmp[1] = _Q15cosPI(thetaTmp[1]);
thetaTmp[2] = _Q15cosPI(thetaTmp[2]);
VectorScale(3, mPLLVabc, thetaTmp, gridVoltEm);
}
float randomNoise()
{
return ((rand() % 1000) / 1000.0f * 2.0f - 1.0f) * 0.05f;
}
void debug_Fake_Three_phase()
{
static const float Ts = 1 / 40e3;
static const float A = 0.9;
static float wt = 0;
float tmp1 = A * cos(wt) + randomNoise();
float tmp2 = A * cos(wt - 2.094395) + randomNoise();
float tmp3 = A * cos(wt + 2.094395) + randomNoise();
mGridAVolt = floatToFract(tmp1);
mGridBVolt = floatToFract(tmp2);
mGridCVolt = floatToFract(tmp3);
wt += Ts * 3.1415926 * 2 * 400;
}
int main()
{
f = fopen("PLLtest.txt", "w");
if (f == NULL)
return 1;
float time = 0;
for (size_t i = 0; i < 1000; i++)
{
time += 1 / fs;
debug_Fake_Three_phase();
PLL_step();
fprintf(f, "%f %f %f %f %f %f %f\n",
time,
fractToFloat(mGridAVolt),
fractToFloat(mGridBVolt),
fractToFloat(mGridCVolt),
fractToFloat(mPLLVabc[0]),
fractToFloat(mPLLVabc[1]),
fractToFloat(mPLLVabc[2]));
printf("Em = %f, Omega = %f\n", fractToFloat(gridVoltEm), fractToFloat(gridVoltFreq) * fs / 2);
}
fclose(f);
return 0;
}fit.h
#ifndef __FIT_H__
#define __FIT_H__
#include "stdint.h"
#include "stdio.h"
#include "math.h"
typedef int16_t fractional;
// 注：下面这个在实际开发中不会用到，只是用于避免运算溢出
typedef int32_t fractAccum;
typedef fractional _Q15;
// 注：此转换不存在溢出
#define fractToFloat(x) ((float)(x) / 32768.0f)
#define floatToFract(x) ((fractional)((x) * 32768.0f))
// -------------------------------------------------------------------------------------
// 部分常数 变量命名中的"d"表示“除以”
#define Q15Const_0 0x0000
#define Q15Const_1 0x7FFF
#define Q15POSMAX 0x7FFF
#define Q15NEGMAX 0x8000
#define Q15Const_n1 0x8000
#define Q15Const_1d2 0x4000
#define Q15Const_1d4 0x2000
#define Q15Const_2d3 0x5555
#define Q15Const_1d3 0x2AAB
#define Q15Const_1dsqrt3 0x49E7
static inline _Q15 Q15Multiply_(_Q15 a, _Q15 b)
{
return (_Q15)(((int32_t)a * b) >> 15);
}
static inline uint16_t Q15MulUint16(_Q15 a, uint16_t b)
{
return (uint16_t)(((int32_t)a * b) >> 15);
}
static inline int16_t Q15MulInt16(_Q15 a, int16_t b)
{
return (int16_t)(((int32_t)a * b) >> 15);
}
static inline fractAccum fractAccumMultiply(fractAccum a, fractAccum b)
{
return (((a * b) >> 15));
}
static inline fractional fractAccumToQ15(fractAccum val)
{
if (val > 0 && val > Q15POSMAX)
{
return Q15POSMAX;
}
else if (val < 0 && -val > Q15NEGMAX)
{
return Q15NEGMAX;
}
return (fractional)val;
}
static inline fractional floatToQ15AutoSaturation(float num)
{
if (num > 0.9996f)
{
return Q15POSMAX;
}
else if (num < -1)
{
return Q15NEGMAX;
}
return floatToFract(num);
}
// static inline fractional
// -------------------- DSP ----------------------
static inline void VectorScale(int num, fractional *dst, fractional *src, fractional scale)
{
for (int i = 0; i < num; i++)
{
dst[i] = Q15Multiply_(src[i], scale);
}
}
static inline fractional VectorDotProduct(int num, fractional *srcV1, fractional *srcV2)
{
fractAccum accum = 0;
for (int i = 0; i < num; i++)
{
accum += fractAccumMultiply(srcV1[i], srcV2[i]);
}
return fractAccumToQ15(accum);
}
static inline fractional VectorPower(int num, fractional *src)
{
fractAccum accum = 0;
for (int i = 0; i < num; i++)
{
accum += fractAccumMultiply(src[i], src[i]);
}
return fractAccumToQ15(accum);
}
static inline fractional* VectorAdd(int num, fractional *dst, fractional *srcV1, fractional *srcV2)
{
for (int i = 0; i < num; i++)
{
dst[i] = fractAccumToQ15((fractAccum)srcV1[i] + (fractAccum)srcV2[i]);
}
return dst;
}
// ------------------- Q15 ----------------------
static inline _Q15 _Q15abs(_Q15 num)
{
if (num > 0)
return num;
return -num;
}
static inline _Q15 _Q15cosPI(_Q15 val)
{
float result = cosf(fractToFloat(val) * 3.1415926f);
return floatToQ15AutoSaturation(result);
}
static inline _Q15 _Q15sinPI(_Q15 val)
{
float result = sinf(fractToFloat(val) * 3.1415926f);
return floatToQ15AutoSaturation(result);
}
static inline _Q15 _Q15sqrt(_Q15 num)
{
return floatToFract(sqrtf(fractToFloat(num)));
}
// --------- PID -----------
typedef struct
{
fractional *PIparam;
fractional *controlHistory;
fractional controlOutput;
fractional measuredOutput;
fractional controlReference;
} tPID;
static inline void PID(tPID *pids)
{
fractAccum Kp = pids->PIparam[0];
fractAccum Ki = pids->PIparam[1];
fractAccum err = pids->controlReference - pids->measuredOutput;
pids->controlHistory[1] = pids->controlHistory[0];
pids->controlHistory[0] = fractAccumToQ15(err);
fractAccum preOut = pids->controlOutput +
fractAccumMultiply(Kp, pids->controlHistory[0]) -
fractAccumMultiply(Kp, pids->controlHistory[1]) +
fractAccumMultiply(Ki, pids->controlHistory[0]);
pids->controlOutput = fractAccumToQ15(preOut);
}
#endif