FlightControl/PID.c at master · wuyingnan/FlightControl · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
#include"PID.h"
#include"TimerA1.h"

unsigned char PID_compute(PID_struct *pid)
{
   if(!pid->inAuto) return 1;
   unsigned long now = TimeBase;
   unsigned long timeChange = (now - pid->lastTime);
   if(timeChange >= pid->SampleTime)
   {
      /*Compute all the working error variables*/
      signed long input = *pid->myInput;
      signed long error = pid->mySetpoint - input;
      pid->ITerm += pid->ki * error /Accuracy;
      if(pid->ITerm > pid->outMax) pid->ITerm= pid->outMax;
      else if(pid->ITerm < pid->outMin) pid->ITerm= pid->outMin;
      signed long dInput = (input - pid->lastInput);

      /*Compute PID Output*/
      signed long output = (pid->kp * error)/Accuracy + pid->ITerm- (pid->kd * dInput)/Accuracy;

      if(output > pid->outMax) output = pid->outMax;
      else if(output < pid->outMin) output = pid->outMin;

      pid->myOutput = output;

      /*Remember some variables for next time*/
      pid->lastInput = input;
      pid->lastTime = now;
      return 0;
   }
   else return 1;
}

void PID_setTunings(PID_struct *pid, unsigned long Kp, unsigned long Ki, unsigned long Kd)
{

   double SampleTimeInSec = ((double)pid->SampleTime)/1000;
   pid->kp = Kp;
   pid->ki = (signed long)(Ki * SampleTimeInSec);
   pid->kd = (signed long)(Kd / SampleTimeInSec);

  if(pid->controllerDirection == REVERSE)
   {
      pid->kp = - pid->kp;
      pid->ki = - pid->ki;
      pid->kd = - pid->kd;
   }
}

void PID_setSampleTime(PID_struct *pid, unsigned int NewSampleTime)
{
   if (NewSampleTime > 0)
   {
      double ratio  = (double)NewSampleTime
                      / (double)pid->SampleTime;
      pid->ki = (signed long)(pid->ki*ratio);
      pid->kd = (signed long)(pid->kd/ratio);
      pid->SampleTime = NewSampleTime;
   }
}

void PID_setOutputLimits(PID_struct *pid, signed long Min, signed long Max)
{
   if(Min >= Max) return;

   pid->outMin = Min;
   pid->outMax = Max;

   if(pid->inAuto)
   {
	   if(pid->myOutput > pid->outMax) pid->myOutput = pid->outMax;
	   else if(pid->myOutput < pid->outMin) pid->myOutput = pid->outMin;

	   if(pid->ITerm > pid->outMax) pid->ITerm= pid->outMax;
	   else if(pid->ITerm < pid->outMin) pid->ITerm= pid->outMin;
   }
}

void PID_setMode(PID_struct *pid, unsigned char Mode)
{
    unsigned char newAuto = (Mode == AUTOMATIC);
    if(newAuto == !pid->inAuto)
    {  /*we just went from manual to auto*/
        PID_init(pid);
    }
    pid->inAuto = newAuto;
}

void PID_init(PID_struct *pid)
{
   pid->ITerm = 0;
   pid->lastInput = *pid->myInput;
   if(pid->ITerm > pid->outMax) pid->ITerm = pid->outMax;
   else if(pid->ITerm < pid->outMin) pid->ITerm = pid->outMin;
}

void PID_setControllerDirection(PID_struct *pid, unsigned char Direction)
{
   if(pid->inAuto && Direction !=pid->controllerDirection)
   {
      pid->kp = -pid->kp;
      pid->ki = -pid->ki;
      pid->kd = -pid->kd;
   }
   pid->controllerDirection = Direction;
}