root / quad1 / AeroQuad / PID.h @ 9240aaa3
History | View | Annotate | Download (1.52 KB)
1 | 9240aaa3 | Alex | /*
|
---|---|---|---|
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 |