Statistics
| Branch: | Revision:

root / quad1 / AeroQuad / PID.h @ 9240aaa3

History | View | Annotate | Download (1.52 KB)

1
/*
2
  AeroQuad v2.1 - October 2010
3
  www.AeroQuad.com
4
  Copyright (c) 2010 Ted Carancho.  All rights reserved.
5
  An Open Source Arduino based multicopter.
6
 
7
  This program is free software: you can redistribute it and/or modify 
8
  it under the terms of the GNU General Public License as published by 
9
  the Free Software Foundation, either version 3 of the License, or 
10
  (at your option) any later version. 
11

12
  This program is distributed in the hope that it will be useful, 
13
  but WITHOUT ANY WARRANTY; without even the implied warranty of 
14
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 
15
  GNU General Public License for more details. 
16

17
  You should have received a copy of the GNU General Public License 
18
  along with this program. If not, see <http://www.gnu.org/licenses/>. 
19
*/
20

    
21
// Modified from http://www.arduino.cc/playground/Main/BarebonesPIDForEspresso
22
float updatePID(float targetPosition, float currentPosition, struct PIDdata *PIDparameters) {
23
  float error;
24
  float dTerm;
25

    
26
  error = targetPosition - currentPosition;
27
  
28
  PIDparameters->integratedError += error * G_Dt;
29
  PIDparameters->integratedError = constrain(PIDparameters->integratedError, -windupGuard, windupGuard);
30
  
31
  dTerm = PIDparameters->D * (currentPosition - PIDparameters->lastPosition);
32
  PIDparameters->lastPosition = currentPosition;
33
  return (PIDparameters->P * error) + (PIDparameters->I * (PIDparameters->integratedError)) + dTerm;
34
}
35

    
36
void zeroIntegralError() {
37
  for (axis = ROLL; axis < LASTLEVELAXIS; axis++)
38
    PID[axis].integratedError = 0;
39
}
40