PID算法验证

算法：

struct PID {
    float kp;
    float kpnfac;
    float ki;
    float kinfac;
    float kd;
};

float gCurPPM = 1300;

float getCurppm() { return gCurPPM; }
float getOut(PID& pid, float dppm)
{
    static float disum = 0;
    static float diffLast = 0;

    float cppm = getCurppm();
    float diff = dppm-cppm;
    float dp = diff*pid.kp;
    float di = diff*pid.ki;

    if (diff<0) {
        dp *= pid.kpnfac;
        di *= pid.kinfac;
    }
    disum += di;

    float out = dp + disum + (diff-diffLast)*pid.kd;
    diffLast = diff;

    printf("Cur=%4.0f dest=%4.0f, sumi=%4.1f out=%4.0f\n", cppm, dppm, disum, out);
    return out;
}

验证代码：

#define DEFAULT_PTERM			0.25f
#define DEFAULT_PNFAC			10.0f
#define DEFAULT_ITERM			0.0006f
#define DEFAULT_INFAC			0.15f

int main()
{
    PID pid;

    pid.kp = DEFAULT_PTERM;
    pid.kpnfac = DEFAULT_PNFAC;
    pid.ki = DEFAULT_ITERM;
    pid.kinfac = DEFAULT_INFAC;
    pid.kd = -0.001;

	for(int i=0;i <1000; i++){
        float out = getOut(pid, 1900);
        gCurPPM += out*2/3;
	}
   return 0;
}

验证输出：

Cur=1300 dest=1900, sumi= 0.4 out= 150
Cur=1400 dest=1900, sumi= 0.7 out= 126
Cur=1484 dest=1900, sumi= 0.9 out= 105
Cur=1554 dest=1900, sumi= 1.1 out=  88
Cur=1612 dest=1900, sumi= 1.3 out=  73
Cur=1661 dest=1900, sumi= 1.4 out=  61
Cur=1702 dest=1900, sumi= 1.6 out=  51
Cur=1736 dest=1900, sumi= 1.7 out=  43
Cur=1764 dest=1900, sumi= 1.7 out=  36
Cur=1788 dest=1900, sumi= 1.8 out=  30
Cur=1808 dest=1900, sumi= 1.9 out=  25
Cur=1825 dest=1900, sumi= 1.9 out=  21
Cur=1838 dest=1900, sumi= 1.9 out=  17
Cur=1850 dest=1900, sumi= 2.0 out=  14
Cur=1860 dest=1900, sumi= 2.0 out=  12
Cur=1868 dest=1900, sumi= 2.0 out=  10
Cur=1874 dest=1900, sumi= 2.0 out=   8
Cur=1880 dest=1900, sumi= 2.0 out=   7
Cur=1885 dest=1900, sumi= 2.0 out=   6
Cur=1889 dest=1900, sumi= 2.1 out=   5
Cur=1892 dest=1900, sumi= 2.1 out=   4
Cur=1895 dest=1900, sumi= 2.1 out=   3
Cur=1897 dest=1900, sumi= 2.1 out=   3
Cur=1899 dest=1900, sumi= 2.1 out=   2
Cur=1900 dest=1900, sumi= 2.1 out=   1
Cur=1901 dest=1900, sumi= 2.1 out=  -1
Cur=1901 dest=1900, sumi= 2.1 out=   0
Cur=1901 dest=1900, sumi= 2.1 out=  -0
Cur=1901 dest=1900, sumi= 2.1 out=   0
Cur=1901 dest=1900, sumi= 2.1 out=  -0
Cur=1901 dest=1900, sumi= 2.1 out=   0
Cur=1901 dest=1900, sumi= 2.1 out=  -0
Cur=1901 dest=1900, sumi= 2.1 out=   0
Cur=1901 dest=1900, sumi= 2.1 out=  -0
Cur=1901 dest=1900, sumi= 2.1 out=   0
Cur=1901 dest=1900, sumi= 2.1 out=  -0
Cur=1901 dest=1900, sumi= 2.1 out=   0
Cur=1901 dest=1900, sumi= 2.1 out=  -0