Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

Augment/improve PID Internals

Browse source
This commit is contained in:
Petr Ledvina 2015-03-18 19:51:14 +01:00
parent 3e8ce5833f
commit e15322868e
1 changed files with 29 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

39
docs/development/PID Internals.md
View file

@ -6,7 +6,8 @@
`rcCommand` - `<-500 - 500>` nominal, but is scaled with `rcRate/100`, max +-1250 `rcCommand` - `<-500 - 500>` nominal, but is scaled with `rcRate/100`, max +-1250
`inclination` - in 0.1 degree, default 50 degrees (500) `inclination` - in 0.1 degree, roll and pitch deviation from horizontal position
`max_angle_inclination` - in 0.1 degree, default 50 degrees (500)
`axisPID` - output to mixer, will be added to throttle(`<1000-2000>`), output range is `<minthrottle, maxthrottle>` (default `<1150 - 1850>`) `axisPID` - output to mixer, will be added to throttle(`<1000-2000>`), output range is `<minthrottle, maxthrottle>` (default `<1150 - 1850>`)
@ -16,14 +17,14 @@
#### Leveling term #### Leveling term
``` ```
error = constrain(2*rcCommand[axis], limit +- max_angle_inclination) - inclination[axis] error = constrain(2*rcCommand[axis], limit +- max_angle_inclination) - inclination[axis]
P = constrain(P8[PIDLEVEL]/100 * error, limit +- 5 * D8[PIDLEVEL]) Pacc = constrain(P8[PIDLEVEL]/100 * error, limit +- 5 * D8[PIDLEVEL])
I = intergrate(error, limit +-10000) * I8[PIDLEVEL] / 4096 Iacc = intergrate(error, limit +-10000) * I8[PIDLEVEL] / 4096
``` ```
#### Gyro term #### Gyro term
``` ```
P = rcCommand[axis]; Pgyro = rcCommand[axis];
error = rcCommand[axis] * 10 * 8 / pidProfile->P8[axis] - gyroData[axis] / 4; (conversion so that error is in deg/s ?) error = rcCommand[axis] * 10 * 8 / pidProfile->P8[axis] - gyroData[axis] / 4; (conversion so that error is in deg/s ?)
I = integrate(error, limit +-16000) / 10 / 8 * I8[axis] / 100 (conversion back to mixer units ?) Igyro = integrate(error, limit +-16000) / 10 / 8 * I8[axis] / 100 (conversion back to mixer units ?)
``` ```
reset I term if reset I term if
@ -33,14 +34,30 @@ reset I term if
##### Mode dependent mix(yaw is always from gyro) ##### Mode dependent mix(yaw is always from gyro)
- HORIZON - proportionally according to max deflection - HORIZON - proportionally according to max deflection
```
- gyro - gyro term deflection = MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])) / 500 ; limit to 0.0 .. 1.0
P = Pacc * (1-deflection) + Pgyro * deflection
- ANGLE - leveling term I = Iacc * (1-deflection) + Igyro * deflection
```
- gyro
```
P = Pgyro
I = Igyro
```
- ANGLE
```
P = Pacc
I = Iacc
```
#### Gyro stabilization #### Gyro stabilization
``` ```
P = -gyroData[axis] / 4 * dynP8 / 10 / 8 P -= gyroData[axis] / 4 * dynP8 / 10 / 8
D = -mean(diff(gyroData[axis] / 4), over 3 samples) * 3 * dynD8 / 32 D = -mean(diff(gyroData[axis] / 4), over 3 samples) * 3 * dynD8 / 32
[equivalent to :]
D = - (gyroData[axis]/4 - (<3 loops old>gyroData[axis]/4)) * dynD8 / 32
``` ```
This can be seen as sum of
- PI controller (handles rcCommand, HORIZON/ANGLE); `Igyro` is only output based on gyroData
- PD controller(parameters dynP8/dynD8) with zero setpoint acting on gyroData